bio_ik
1.0.0
Этот репозиторий предоставляет автономную реализацию различных методов оптимизации под лицензией BSD для эффективного решения обобщенных обратных задач кинематики.
Весь модуль был реализован Филиппом Руппелем в рамках его магистерской диссертации.
Для повторной реализации исходного алгоритма BioIK на основе C++, который изначально продавался в магазине Unity, вы можете использовать режим bio1 , отличный от режима по умолчанию. Режим по умолчанию bio2_memetic не имеет общего кода с этой реализацией, было показано, что он превосходит его с точки зрения успешности, точности и эффективности и фактически может использоваться для точных роботизированных приложений [4].
Вам понадобится версия ROS Indigo или новее (wiki.ros.org). Программное обеспечение было разработано на Ubuntu Linux 16.04 LTS с ROS Kinetic, но также было протестировано на Ubuntu Linux 14.04 LTS с ROS Indigo. Более новые версии ROS должны работать, но, возможно, потребуется некоторая адаптация. Ниже приведены инструкции для конкретной версии.
Загрузите пакет bio_ik и распакуйте его в рабочее пространство Catkin.
Запустите catkin_make , чтобы скомпилировать рабочую область:
roscd
cd src
git clone https://github.com/TAMS-Group/bio_ik.git
roscd
catkin_make
Настройте Moveit для использования bio_ik в качестве решателя кинематики (см. следующий раздел).
Используйте Moveit для планирования и выполнения движений или используйте свой собственный код вместе с узлом move_group для перемещения вашего робота.
Как обычно, общедоступный API указан в файлах общедоступных заголовков пакета bio_ik , расположенных в подкаталоге include/bio_ik ; исходники, включая несколько частных заголовочных файлов, находятся в подкаталоге src .
Для простоты использования и совместимости с существующим кодом алгоритм bio_ik инкапсулирован в виде плагина кинематики Moveit. Таким образом, bio_ik можно использовать в качестве прямой замены стандартного решателя IK на базе Orocos/KDL. Учитывая имя конечного эффектора и целевую позу с 6 степенями свободы, bio_ik будет искать действительную конфигурацию сустава робота, которая достигает заданной цели.
В наших тестах (см. ниже), как по показателю успешности, так и по времени решения, bio_ik регулярно превосходил решатель Orocos [1] и конкурировал с trac-ik [2]. Алгоритм bio_ik также можно использовать для систем с высокой степенью резкости, таких как роботы-змеи, и он автоматически сходится к лучшим приближенным решениям для рук с низкой степенью свободы, где некоторые целевые позы точно недостижимы.
Хотя вы можете написать файлы конфигурации Moveit вручную, самый простой способ — запустить помощник настройки Moveit для вашего робота, а затем выбрать bio_ik в качестве решателя IK при настройке конечных эффекторов. После настройки решатель можно вызвать с помощью стандартного API Moveit или использовать в интерактивном режиме из rviz с помощью подключаемого модуля MotionPlanning GUI.
Убедитесь, что у вас есть модель URDF (или xacro) для вашего робота.
Запустите помощник настройки moveit, чтобы создать файлы конфигурации Moveit:
rosrun moveit_setup_assistant moveit_setup_assistant
Ассистент настройки автоматически ищет все доступные плагины решателя ИК в вашем рабочем пространстве. Таким образом, вы можете просто выбрать bio_ik в качестве решателя ИК из раскрывающегося списка для каждого исполнительного органа, а затем настроить параметры кинематики, а именно точность положения по умолчанию (в метрах) и время ожидания (в секундах). Для типичных рычагов с 6 или 7 степенями свободы достаточно точности 0,001 м (или меньше) и времени ожидания 1 мс. Более сложным роботам может потребоваться более длительный тайм-аут.
Создайте файлы конфигурации moveit с помощью помощника установки. Конечно, вы также можете редактировать файл конфигурации config/kinematics.yaml в своем любимом текстовом редакторе. Например, конфигурация робота PR2 может выглядеть так:
# example kinematics.yaml for the PR2 robot
right_arm:
# kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
# kinematics_solver_attempts: 1
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 1
left_arm:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 1
all:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.02
kinematics_solver_attempts: 1
# optional bio_ik configuration parameters
# center_joints_weight: 1
# minimal_displacement_weight: 1
# avoid_joint_limits_weight: 1
Для первого теста запустите демонстрационный запуск, созданный Moveit. После запуска rviz включите плагин планирования движения, затем выберите один из конечных исполнительных механизмов вашего робота. Rviz должен показать шестимерный интерактивный маркер (положение и ориентация) для выбранных концевых эффекторов. Перемещайте интерактивный маркер и смотрите, как bio_ik вычисляет позы для вашего робота.
Если вы также установили демо-версию bio_ik (см. ниже), вы сможете запустить одну из предустановленных демо-версий:
roslaunch pr2_bioik_moveit demo.launch
roslaunch pr2_bioik_moveit valve.launch
roslaunch pr2_bioik_moveit dance.launch
Теперь вы готовы использовать bio_ik из своих программ на C/C++ и Python, используя стандартный API Moveit. Чтобы явно запросить решение IK на C++:
robot_model_loader::RobotModelLoader robot_model_loader(robot);
auto robot_model = robot_model_loader.getModel();
auto joint_model_group = robot_model->getJointModelGroup(group);
auto tip_names = joint_model_group->getSolverInstance()->getTipFrames();
kinematics::KinematicsQueryOptions opts;
opts.return_approximate_solution = true; // optional
robot_state::RobotState robot_state_ik(robot_model);
// traditional "basic" bio_ik usage. The end-effector goal poses
// and end-effector link names are passed into the setFromIK()
// call. The KinematicsQueryOptions are empty.
//
bool ok = robot_state_ik.setFromIK(
joint_model_group, // joints to be used for IK
tip_transforms, // multiple end-effector goal poses
tip_names, // names of the end-effector links
attempts, timeout, // solver attempts and timeout
moveit::core::GroupStateValidityCallbackFn(),
opts // mostly empty
);
Для многих приложений роботов важно указать больше, чем просто одну позу рабочего органа. Типичные примеры включают в себя
В bio_ik такие задачи определяются как комбинация нескольких отдельных целей .
Затем алгоритм пытается найти конфигурацию робота, которая одновременно удовлетворяет всем поставленным целям, минимизируя функцию квадратичной ошибки, построенную на основе взвешенных отдельных целей. Хотя текущий API Moveit не поддерживает задачи с несколькими целями напрямую, он предоставляет класс KinematicQueryOptions. Таким образом, bio_ik просто предоставляет набор предопределенных целей движения, а комбинация заданных пользователем целей передается через Moveit в решатель IK. Никаких изменений API в Moveit не требуется, но использование решателя IK теперь заключается в передаче взвешенных целей через KinematicQueryOptions. К заранее определенным целям относятся:
Чтобы решить проблему движения вашего робота, задача теперь состоит в том, чтобы создать подходящую комбинацию отдельных целей.
В следующем примере мы хотим захватить, а затем медленно повернуть колесо клапана с помощью левого и правого захватов робота PR2:
bio_ik::BioIKKinematicsQueryOptions ik_options;
ik_options.replace = true;
ik_options.return_approximate_solution = true;
auto* ll_goal = new bio_ik::PoseGoal();
auto* lr_goal = new bio_ik::PoseGoal();
auto* rl_goal = new bio_ik::PoseGoal();
auto* rr_goal = new bio_ik::PoseGoal();
ll_goal->setLinkName("l_gripper_l_finger_tip_link");
lr_goal->setLinkName("l_gripper_r_finger_tip_link");
rl_goal->setLinkName("r_gripper_l_finger_tip_link");
rr_goal->setLinkName("r_gripper_r_finger_tip_link");
ik_options.goals.emplace_back(ll_goal);
ik_options.goals.emplace_back(lr_goal);
ik_options.goals.emplace_back(rl_goal);
ik_options.goals.emplace_back(rr_goal);
Мы также поставили перед собой пару второстепенных целей. Во-первых, мы хотим, чтобы головка PR2 смотрела в центр клапана. Во-вторых, мы хотим, если это возможно, избежать ограничений на все суставы. В-третьих, мы хотим, чтобы решения ИК были как можно ближе к предыдущей конфигурации суставов, то есть небольшими и эффективными движениями. Это решается добавлением MinimalDisplacementGoal. В-четвертых, мы хотим избежать подъемов туловища, которые на PR2 очень медленны. Все это указывается легко:
auto* lookat_goal = new bio_ik::LookAtGoal();
lookat_goal->setLinkName("sensor_mount_link");
ik_options.goals.emplace_back(lookat_goal);
auto* avoid_joint_limits_goal = new bio_ik::AvoidJointLimitsGoal();
ik_options.goals.emplace_back(avoid_joint_limits_goal);
auto* minimal_displacement_goal = new bio_ik::MinimalDisplacementGoal();
ik_options.goals.emplace_back(minimal_displacement_goal);
auto* torso_goal = new bio_ik::PositionGoal();
torso_goal->setLinkName("torso_lift_link");
torso_goal->setWeight(1);
torso_goal->setPosition(tf::Vector3( -0.05, 0, 1.0 ));
ik_options.goals.emplace_back(torso_goal);
Для фактического поворотного движения мы вычисляем набор необходимых поз захвата в цикле:
for(int i = 0; ; i++) {
tf::Vector3 center(0.7, 0, 1);
double t = i * 0.1;
double r = 0.1;
double a = sin(t) * 1;
double dx = fmin(0.0, cos(t) * -0.1);
double dy = cos(a) * r;
double dz = sin(a) * r;
tf::Vector3 dl(dx, +dy, +dz);
tf::Vector3 dr(dx, -dy, -dz);
tf::Vector3 dg = tf::Vector3(0, cos(a), sin(a)) * (0.025 + fmin(0.025, fmax(0.0, cos(t) * 0.1)));
ll_goal->setPosition(center + dl + dg);
lr_goal->setPosition(center + dl - dg);
rl_goal->setPosition(center + dr + dg);
rr_goal->setPosition(center + dr - dg);
double ro = 0;
ll_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
lr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
rl_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
rr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
lookat_goal->setAxis(tf::Vector3(1, 0, 0));
lookat_goal->setTarget(rr_goal->getPosition());
// "advanced" bio_ik usage. The call parameters for the end-effector
// poses and end-effector link names are left empty; instead the
// requested goals and weights are passed via the ik_options object.
//
robot_state.setFromIK(
joint_model_group, // active PR2 joints
EigenSTL::vector_Affine3d(), // no explicit poses here
std::vector<std::string>(), // no end effector links here
0, 0.0, // take values from YAML file
moveit::core::GroupStateValidityCallbackFn(),
ik_options // four gripper goals and secondary goals
);
... // check solution validity and actually move the robot
}
Когда вы выполните код, PR2 достанет колесо клапана и повернет его. Время от времени он не может добраться до клапана с текущей конфигурацией рычага и снова захватывает колесо.
Дополнительные примеры см. в [3] и [4].
BioIK был разработан для эффективного поиска хороших решений невыпуклых обратных задач кинематики с несколькими целями и локальными минимумами. Однако для некоторых приложений это может привести к неинтуитивным результатам. Если существует несколько возможных решений данной проблемы ИК и если пользователь явно не указал, какое из них выбрать, результат может быть выбран случайным образом из набора всех допустимых решений. При постепенном отслеживании декартовой траектории это может привести к нежелательным скачкам, тряске и т. д. Чтобы постепенно генерировать плавную траекторию с помощью BioIK, желаемое поведение должно быть указано явно, что можно сделать двумя способами.
BioIK предлагает ряд различных решателей, включая глобальные оптимизаторы и локальные оптимизаторы. По умолчанию BioIK использует меметический глобальный оптимизатор ( bio2_memetic ). Другой класс решателя можно выбрать, установив параметр mode в файле kinematics.yaml конфигурации вашего робота MoveIt.
Пример:
all:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.02
kinematics_solver_attempts: 1
mode: gd_c
Доступные на данный момент локальные оптимизаторы:
gd, gd_2, gd_4, gd_8
gd_r, gd_r_2, gd_r_4, gd_r_8
gd_c, gd_c_2, gd_c_4, gd_c_8
jac, jac_2, jac_4, jac_8
Соглашение об именовании: <solver type>_[<variant>_]<number of threads>
Примечания:
gd_* поддерживают произвольные типы целей.jac_* поддерживают только цели постановки, но теоретически в некоторых случаях могут быть более стабильными.gd_c обычно превосходили другие локальные решатели.gd_c , gd или jac ).Вы можете заставить глобальный оптимизатор возвращать локальный минимум посредством регуляризации.
Для конкретного случая дополнительных движений робота (так называемого бега трусцой ) самым простым решением является указание как PoseGoal , так и специального RegularizationGoal , который пытается сохранить решение IK суставного пространства как можно ближе к заданной исходной конфигурации робота. Обычно вы используете высокий вес для PoseGoal и меньший вес для регуляризатора.
Вы также можете добавить MinimalDisplacementGoal вместо RegularizationGoal . Обе цели пытаются сохранить решение IK близким к текущему состоянию робота-сеятеля, но немного различаются в обработке быстрых и медленных суставов робота (например, PR2 имеет быстрые суставы рук и довольно медленный шарнир подъема туловища). Возможно, вы захотите поиграть с обеими целями, чтобы увидеть, какая из них лучше соответствует вашим потребностям.
Некоторые контроллеры промышленных роботов на манипуляторах с 7 степенями свободы ведут себя так, как будто они работают на манипуляторе с 6 степенями свободы и одним дополнительным суставом. Обычно можно указать значение дополнительного сустава, а затем искать решение IK для оставшихся шести суставов. Этого поведения можно добиться в bio_ik путем объединения PoseGoal для конечного эффектора с JointPositionGoal для одного (любого) сустава робота.
Еще один полезный прием — стараться удерживать суставы робота по центру, поскольку это позволит роботу (суставам) двигаться в обоих направлениях. Просто объедините PoseGoal с CenterJointsGoal и, при необходимости, с RegularizationGaol .
Вы также можете комбинировать регуляризацию с локальным решателем gd_* .
Решатель bio_ik основан на меметическом алгоритме, который сочетает оптимизацию на основе градиента с генетической оптимизацией и оптимизацией роя частиц.
Внутри системы векторы всех значений суставов робота используются для кодирования различных промежуточных решений ( генотипа генетического алгоритма). Во время оптимизации значения соединений всегда проверяются на соответствие активным нижним и верхним пределам соединений, поэтому генерируются только действительные конфигурации робота.
Для расчета приспособленности людей рассчитывается совокупная ошибка по всем заданным индивидуальным целям. Любая особь с нулевой ошибкой является точным решением задачи ИК, а особи с малой ошибкой соответствуют приближенным решениям.
Индивидуумы сортируются по их приспособленности, а оптимизация на основе градиента проверяется на нескольких лучших конфигурациях, что приводит к быстрой сходимости и хорошей производительности для решения многих задач. Если в результате оптимизации на основе градиента решение не найдено, новые особи создаются с помощью набора операторов мутации и рекомбинации, что приводит к хорошему исследованию пространства поиска.
Более подробную информацию см. в [3] и [4]. См. [5] и [6] для подробного объяснения более раннего эволюционного алгоритма анимации персонажей видеоигр.
Мы протестировали bio_ik на множестве различных манипуляторов роботов, используя как традиционный API с одним конечным исполнительным механизмом, так и расширенный API с несколькими конечными рабочими органами, основанный на KinematicsQueryOptions.
Одно простое самотестирование состоит из генерации случайных действительных конфигураций робота, выполнения прямой кинематики для расчета результирующего положения рабочего органа и запроса плагина IK для поиска подходящей конфигурации соединения робота. Затем успех проверяется путем повторного запуска прямой кинематики и проверки того, что положение конечного эффектора для сгенерированного решения IK соответствует целевому положению. Этот подход можно легко использовать для тысяч или миллионов случайных поз, он производит выборку всего рабочего пространства робота и позволяет быстро генерировать оценки успеха и времени решения для выбранного решателя IK.
Конечно, запуск тестов требует установки соответствующих моделей роботов и добавляет множество зависимостей. Поэтому эти тесты не входят в стандартный пакет bio_ik, а упаковываются отдельно.
Для удобства мы предоставляем пакет pr2_bioik_moveit , который также включает в себя несколько демонстраций bio_ik для сервисного робота PR2. Это демонстрации только кинематики; но, конечно, вы также можете попробовать запустить демо-версии на реальном роботе (если он у вас есть) или на симуляторе Gazebo (если вы установили Gazebo).
Просто клонируйте пакет описания PR2 (внутри pr2_common ) и пакет pr2_bioik_moveit в рабочее пространство Catkin:
roscd
cd src
git clone https://github.com/PR2/pr2_common.git
git clone https://github.com/TAMS-Group/bioik_pr2.git
catkin_make
Для теста производительности FK-IK-FK запустите
roslaunch pr2_bioik_moveit env_pr2.launch
roslaunch pr2_bioik_moveit test_fk_ik.launch
... // wait for test completion and results summary
Кинематика и динамика Орокос, http://www.orocos.org
П. Бисон и Б. Эймс, TRAC-IK: Библиотека с открытым исходным кодом для улучшенного решения общей обратной кинематики , Материалы конференции IEEE RAS Humanoids Conference, Сеул, Корея, ноябрь 2015 г.
Филипп Руппель, Норман Хендрих, Себастьян Старк, Цзяньвэй Чжан, Функции стоимости для определения движений всего тела и задач многоцелевого манипулирования , Международная конференция IEEE по робототехнике и автоматизации (ICRA-2018), Брисбен, Австралия. DOI: 10.1109/ICRA.2018.8460799
Филипп Руппель, Оптимизация производительности и реализация эволюционной обратной кинематики в ROS , магистерская диссертация, Гамбургский университет, 2017 PDF
Себастьян Старк, Норман Хендрич, Цзяньвэй Чжан, Меметический эволюционный алгоритм для шарнирного кинематического движения в реальном времени , IEEE Intl. Конгресс по эволюционным вычислениям (CEC-2017), стр.2437-2479, 4-8 июня 2017 г., Сан-Себастьян, Испания. DOI: 10.1109/CEC.2017.7969605
Себастьян Старк, Норман Хендрич, Деннис Крупке, Цзянвэй Чжан, Многоцелевая эволюционная оптимизация обратной кинематики на шарнирно-сочлененных и гуманоидных роботах , IEEE Intl. Конференция по интеллектуальным роботам и системам (IROS-2017), 24-28 сентября 2017, Ванкувер, Канада
