Задатчики (походки, анимации)

Этот документ описывает типовой интерфейс задатчика движения. Задатчики формируют задающее воздействии для некоторого набора исполнительных органов. Для кинематических цепочек выдается желаемая поза в угловой или декартовой системе координат. В остальных случаях тип задания зависит от реализации.

Разрешение конфликтов ресурсов обеспечивается арбитром и клиентским плагином.

Способ взаимодействия с верхним уровнем не декларируется, но рекомендуется использовать actionlib.

Набор интерфейсов сильно зависит от назначения. Принципиально выделяются три типа задатчиков: 1. Follow реализуют алгоритмы слежения, используют механизм портов для получения задающего воздействия из вне и операции для активации и деактивации. Примеры: HeadFollowPoint (следить за точкой), FollowJointState (следить за позой). 1. Move реализуют алгоритмы дискретного позиционного управления. Используют механизм actionlib для получения задания. Примеры: MoveJointState (переместить в заданную позу). 1. Trajectory реализуют алгоритмы непрерывного программного управления (исполнение известной траектории). Используют механизм actionlib для получения задания. Примеры: TrajectoryJointState (исполнение JointTrajectory).

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

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

Синхронизация

sync (TimerEvent, EventPort) --- синхронизация таймера, по этому сообщению делается шаг по времени и начинается вычисление задающего воздействия.

Текущее состояние робота

joints_sorted (JointState) --- состояние робота в угловой СК (желаемое с предыдущего шага или реальное по датчикам), полное, отсортированная по кинематическим цепочкам.
limbs_sorted (RigidBodyState) --- состояние робота в декартовой СК (желаемое с предыдущего шага или реальное по датчикам), полное (ноги, голова), сортированное по цепочкам.
support_sorted (SupportState) --- список точек касания по сенсорной системе.

Интерфейс высшего уровня (порты используются в компонентах слежения)

joints_ref (JointState) --- желаемое положение звеньев.
twist_ref (Twist) --- желаемая скорость тела или конечности.

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

Задающее воздействие

joints_ref (JointState) --- задающее воздействие в угловой системе координат, неполная поза (предпочтительный вариант). Задает положение головы, ног, хвоста, глаз.
imbs_ref (RigidBodyState) --- задающее воздействие в декартовой системе координат (для походок, для остальных компонент использование нежелательно из-за неоднозначности).
support_ref (SupportState) --- расчетный список касаний (для походок, опционально).
body_ref (RigidBodyState) --- расчетное положение и скорость платформы (для походок, опционально).

Операции

Если компонент использует внешний модуль обратной кинематики, то требует сервис ik, содержащий операции: 1. solveIK(string name, Pose, JntArray) 1. solveIKLimits(string name, Pose in, JntArray out, JntArray min, JntArray max) 1. solveVelIK(string name, JntArray pos, Twist vel_in, JntArray vel_out) Подробнее смотрите в kinematics_inv.

Параметры

Параметры походки и анимаций, настраиваемые один раз при запуске.

period --- период дискретизации, должен совпадать с периодом таймера.

Плагины

Требует загрузки: плагин клиента подсистемы распределив ресурсов и предоставляемые им операции. Опционально требует реализации bool resourceChangeHook() --- пользовательский callbcak, вызываемый при изменении состава ресурсов. Возвращает true, если компонент остается активен.

Операции

Операции активации/деактивации задатчика.

start/stop --- стандартные операции запуска компонента используются для активации/деактивации.
rosSetOperational() (std_srv::SetBool) --- операция, совместимая с ROS, для активации компонента.

Интерфейс `actionlib`

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

Одна цель (предпочтительный вариант):

Предлагается использовать OrocosSimpleActionServer аналог класса SimpleActionServer, скрывающий от пользователя взаимодействия с GoalHandle и клиентом арбитра ресурсов. Он требет от пользовательского кода аналогичный набор callback. 1. newGoalHook(Goal) --- получить задание (цель движения, его параметры). 2. cancelGoalHook() --- прекратить исполнение задания (опционально). 3. resourceChangeHook() --- реакция на изменение состава выделенных ресурсов.

Поддержка нескольких целей:

goalCallback(GoalHandle) --- получить задание (цель движения, его параметры).
cancelCallback(GoalHandle) --- прекратить исполнение задания.
Извещения о завершении, отказе исполнения осуществялется средствами actionlib через взаимодействие с GoalHandle. Управление набором GoalHandle и их сотояниями ложится на пользователя.

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

После исполнения configure компонент готов к работе и запуску.

Переход к активному состоянию (формирует задающее воздействие) осуществляется вызовом actionlib, либо start() (эти механизмы исключают друг друга). Проводится проверка возможности активации в данном состоянии робота, в зависимости от результата формируется отказ (goal->setRejected или возврат false), либо создается запрос ресурсов.

Непосредственный переход в состояние активное состояние и запуск компонента происходит по подтверждению выделения ресурсов (в resourceChangeHook() или путем мониторинга isOperational).

В активном исполняется основной код задатчика с периодом, определяемым таймером. Задаваемая поза out_joints_ref не должна существенно отличаться от текущей in_joints. Этого можно достичь, используя сервисы фильтров, либо учитывая текущую позу при расчете задающей (например, компонент вычисялет смещение). Если компонент формирует программную траекторию, то существенное отклонение текущей позы от задающей можно рассматривать как ошибочную ситуацию и прерывать исполнение компонента.

По завершению движения, либо по требованию освобождения ресурсов производится остановка выполнения. Производится информирование высшего уровня средствами actionlib (цель достигнута/отвергнута), либо иным способом, если это требуется.

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

Для компонентов на базе actionlib следует удалить операцию start() из внешнего интерфейса, либо попытка активации должна приводить к неуспеху.

Состояния "запущен" и "активен" не тождественны. Неактивный компонент не посылает задающее воздействие, независимо от его состояния, а остановленный компонент не обрабатывает сообщения, включая сообщения actionlib. Поэтому для обработки сообщений арбитра и actionlib задатчик должен быть запущен (нежелательно), либо такая обработка должна быть явно разрешена из dataOnPortHook() путем возврата true для соответствующих портов (желательно).

Слишком большое число одновременно запущенных задатчиков может вызвать проблемы. Поэтому разумно запускать задатчик перед запросом ресурсов (из goalCallback()) и останавливать при деактивации (из upadteHook). Однако первичные реализации можно делать на база постоянно запущенных задатчиков.

Пример взаимодействия с клиентским плагином арбитра ресурсов: компонент переключения походок.

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

Исключения

Зависят от реализации.

`OrocosSimpleActionServer`

Класс-обертка для исходной реализации ActionServer для OROCOS. Поддерживает работу с одной целью, скрывает сложности взаимодействия с GoalHandler. Принцип-работы близок к SimpleActionServer ROS в режиме с Callback, однако он предоставляет дополнительные средства доступа к ожидающей активации цели (PENDING).

Интерфейс

start/shutdown --- запуск, остановка.
bool isActive() --- присутствует ли активная цель.
bool isPreemting() --- активная цель ожижает отмены по запросу клиента.
bool isPending() --- присутствует цель, ожидающая активации.
boost::shared_ptr<const Goal> getActiveGoal() --- возвращает активную цель, если есть.
boost::shared_ptr<const Goal> getPendingGoal() --- возвращает ожидающую активации цель, если есть.
bool acceptPending(Result result) --- принять ожидающую активации цель, ничего не делает, если такой цели нет. Меняет статус активной цели (если есть) на "прервана" (preemted) с результатом result.
bool rejectPending(Result result) --- отклонить ожидающую активации цель, ничего не делает, если такой цели нет.
bool abortActive(Result result) --- прервать (aborted) выполнение активной цели, ничего не делает, если такой цели нет.
bool cancelActive(Result result) --- отменить (canceled) выполнение активной цели, ничего не делает, если такой цели нет.
bool succeedActive(Result result) --- завершить выполнение активной цели, ничего не делает, если такой цели нет.
void setGoalHook(boost::function< void()> newGoalHook) --- функция будет вызываться при появлении новой (pending) цели.
void setCancelHook(boost::function< void()> cancelGoalHook) --- функция будет вызываться при отмене активной цели.

Функции типа accept/reject возвращают true, если совершают действие, иначе --- false.

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

Одновременно могут храниться две цели: активная (ACTIVE) и ожидающую активации (PENDING). Активная цель всегда одна. Ожидающая активации заменяет ее по вызовы acceptPending(). Ожидающая цель тоже одна. Новая цель отменяет текущую ожидающую и занимает ее место. По этому событияю вызывается newGoalHook.

Диаграмма состояний OrocosSimpleActionServer:

Пример использования

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

void newGoalHook(pending_goal) {
    if (! условия исполнеия для pending_goal) {
        as.rejectPending(не выполнены условия запуска);
        return; //выполнеие активной цели, если была, продолжается
    }
    if (isOperational() && hasResources(нужные ресурсы)) {
        as.acceptPending(прервана другой целью); % заменяем одну цель на другую без запроса ресурсов.
    }
    else {
        rc.resourceChangeRequest(нужные ресурсы); // запрос набора ресурсов.
    }
}

bool resourceChangeHook() {
    if (hasResources(нужные ресурсы) {
        if (as.isPending()) {
            as.acceptPending(прервана другой целью); // прерываем активную цель, если была
            операции по запуску новой цели
        }
        return true; 
    }
    else {
        rejectPending(нет ресурсов);
        if (as.isActive()) {
             операции по прекращению работы
             as.abortActive(отобраны ресурсы);
        }
        return false;
    }
}

void cancelGoalHook() {
    операции по прекращению работы
    as.abortActive(отменена);
    stopOperational();
}

Задатчики (походки, анимации)

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

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

Операции

Параметры

Плагины

Операции

Интерфейс actionlib

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

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

Исключения

OrocosSimpleActionServer

Интерфейс

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

Пример использования

Интерфейс `actionlib`

`OrocosSimpleActionServer`