bio_ik
1.0.0
Ce référentiel fournit une implémentation autonome sous licence BSD d'une variété de méthodes d'optimisation pour résoudre efficacement des problèmes de cinématique inverse généralisés.
L'ensemble du module a été mis en œuvre par Philipp Ruppel dans le cadre de son mémoire de maîtrise.
Pour une réimplémentation basée sur C++ de l'algorithme "BioIK" original, tel que vendu à l'origine dans la boutique Unity, vous pouvez utiliser le mode non par défaut bio1 . Le mode par défaut bio2_memetic ne partage aucun code avec cette implémentation, il a été démontré qu'il le surpassait en termes de taux de réussite, de précision et d'efficacité, et est réellement utilisable pour des applications robotiques précises [4].
Vous aurez besoin de la version ROS Indigo ou plus récente (wiki.ros.org). Le logiciel a été développé sur Ubuntu Linux 16.04 LTS avec ROS Kinetic, mais a également été testé sur Ubuntu Linux 14.04 LTS avec ROS Indigo. Les versions plus récentes de ROS devraient fonctionner, mais peuvent nécessiter une certaine adaptation. Voir ci-dessous pour les instructions spécifiques à la version.
Téléchargez le package bio_ik et décompressez-le dans votre espace de travail catkin.
Exécutez catkin_make pour compiler votre espace de travail :
roscd
cd src
git clone https://github.com/TAMS-Group/bio_ik.git
roscd
catkin_make
Configurez Moveit pour utiliser bio_ik comme solveur cinématique (voir section suivante).
Utilisez Moveit pour planifier et exécuter des mouvements ou utilisez votre propre code avec le nœud move_group pour déplacer votre robot.
Comme d'habitude, l'API publique est spécifiée dans les fichiers d'en-tête publics du package bio_ik , situés dans le sous-répertoire include/bio_ik ; les sources comprenant quelques fichiers d'en-tête privés se trouvent dans le sous-répertoire src .
Pour plus de facilité d'utilisation et de compatibilité avec le code existant, l'algorithme bio_ik est encapsulé sous forme de plugin cinématique Moveit. Par conséquent, bio_ik peut être utilisé en remplacement direct du solveur IK par défaut basé sur Orocos/KDL. Étant donné le nom d'un effecteur final et une pose cible à 6 DOF, bio_ik recherchera une configuration d'articulation de robot valide qui atteint la cible donnée.
Dans nos tests (voir ci-dessous), tant en termes de taux de réussite que de temps de résolution, bio_ik a régulièrement surpassé le solveur Orocos [1] et est compétitif avec trac-ik [2]. L'algorithme bio_ik peut également être utilisé pour les systèmes à haut DOF comme les serpents robots, et il convergera automatiquement vers les meilleures solutions approximatives pour les bras à faible DOF où certaines poses cibles ne sont pas atteintes exactement.
Bien que vous puissiez écrire les fichiers de configuration Moveit à la main, le moyen le plus simple consiste à exécuter l'assistant de configuration Moveit pour votre robot, puis à sélectionner bio_ik comme solveur IK lors de la configuration des effecteurs finaux. Une fois configuré, le solveur peut être appelé à l'aide de l'API Moveit standard ou utilisé de manière interactive depuis rviz à l'aide du plugin MotionPlanning GUI.
Assurez-vous d'avoir un modèle URDF (ou xacro) pour votre robot.
Exécutez l'assistant de configuration moveit pour créer les fichiers de configuration Moveit :
rosrun moveit_setup_assistant moveit_setup_assistant
L'assistant de configuration recherche automatiquement tous les plug-ins du solveur IK disponibles dans votre espace de travail. Par conséquent, vous pouvez simplement sélectionner bio_ik comme solveur IK dans la liste déroulante pour chaque effecteur final, puis configurer les paramètres cinématiques, à savoir la précision de position par défaut (mètres) et le délai d'attente (en secondes). Pour les bras typiques à 6 ou 7 DOF, une précision de 0,001 m (ou moins) et un délai d'attente de 1 ms devraient suffire. Les robots plus complexes peuvent nécessiter un délai d'attente plus long.
Générez les fichiers de configuration moveit à partir de l'assistant de configuration. Bien entendu, vous pouvez également modifier le fichier de configuration config/kinematics.yaml avec votre éditeur de texte préféré. Par exemple, une configuration pour le robot PR2 pourrait ressembler à ceci :
# 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
Pour un premier test, lancez le lancement de la démo créée par Moveit. Une fois rviz exécuté, activez le plugin de planification de mouvement, puis sélectionnez l'un des effecteurs finaux de votre robot. Rviz doit afficher un marqueur interactif 6-D (position et orientation) pour le ou les effecteurs finaux sélectionnés. Déplacez le marqueur interactif et regardez bio_ik calculer les poses de votre robot.
Si vous avez également installé la démo bio_ik (voir ci-dessous), vous devriez pouvoir exécuter l'une des démos prédéfinies :
roslaunch pr2_bioik_moveit demo.launch
roslaunch pr2_bioik_moveit valve.launch
roslaunch pr2_bioik_moveit dance.launch
Vous êtes maintenant prêt à utiliser bio_ik depuis vos programmes C/C++ et Python, en utilisant l'API standard Moveit. Pour demander explicitement une solution IK en 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
);
Pour de nombreuses applications robotiques, il est essentiel de spécifier plus qu’une simple pose d’effecteur final. Les exemples typiques incluent
Dans bio_ik, ces tâches sont spécifiées comme une combinaison de plusieurs objectifs individuels .
L'algorithme tente ensuite de trouver une configuration de robot qui remplit simultanément tous les objectifs donnés en minimisant une fonction d'erreur quadratique construite à partir des objectifs individuels pondérés. Bien que l'API Moveit actuelle ne prenne pas directement en charge les tâches à objectifs multiples, elle fournit la classe KinematicQueryOptions. Par conséquent, bio_ik fournit simplement un ensemble d'objectifs de mouvement prédéfinis, et une combinaison des objectifs spécifiés par l'utilisateur est transmise via Moveit au solveur IK. Aucune modification de l'API n'est requise dans Moveit, mais l'utilisation du solveur IK consiste désormais à transmettre les objectifs pondérés via les KinematicQueryOptions. Les objectifs prédéfinis comprennent :
Pour résoudre un problème de mouvement sur votre robot, l’astuce consiste désormais à construire une combinaison appropriée d’objectifs individuels.
Dans l'exemple suivant, nous souhaitons saisir puis faire tourner lentement une roue de valve avec les pinces gauche et droite du robot 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);
Nous avons également fixé quelques objectifs secondaires. Premièrement, nous voulons que la tête du PR2 regarde vers le centre de la valve. Deuxièmement, nous voulons éviter, si possible, les limites de joint sur toutes les articulations. Troisièmement, nous souhaitons que les solutions IK soient aussi proches que possible de la configuration de joint précédente, ce qui signifie des mouvements petits et efficaces. Ceci est géré en ajoutant le MinimalDisplacementGoal. Quatrièmement, nous voulons éviter les mouvements de levage du torse, qui sont très lents sur le PR2. Tout cela se précise facilement :
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);
Pour le mouvement de rotation réel, nous calculons un ensemble de poses de pince requises dans une boucle :
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
}
Lorsque vous exécutez le code, le PR2 atteint la roue de valve et la fait tourner. De temps en temps, il ne peut pas atteindre la valve avec sa configuration actuelle de bras et rattrapera la roue.
Voir [3] et [4] pour plus d'exemples.
BioIK a été développé pour trouver efficacement de bonnes solutions aux problèmes de cinématique inverse non convexes avec des objectifs multiples et des minima locaux. Cependant, pour certaines applications, cela peut conduire à des résultats peu intuitifs. S'il existe plusieurs solutions possibles à un problème CI donné, et si l'utilisateur n'a pas explicitement spécifié laquelle choisir, un résultat peut être sélectionné de manière aléatoire parmi l'ensemble de toutes les solutions valides. Lors du suivi incrémentiel d'un chemin cartésien, cela peut entraîner des sauts, des secousses indésirables, etc. Pour générer de manière incrémentielle une trajectoire fluide à l'aide de BioIK, le comportement souhaité doit être spécifié explicitement, ce qui peut être effectué de deux manières.
BioIK propose un certain nombre de solveurs différents, notamment des optimiseurs globaux et des optimiseurs locaux. Par défaut, BioIK utilise un optimiseur global mémétique ( bio2_memetic ). Une classe de solveur différente peut être sélectionnée en définissant le paramètre mode dans le fichier kinematics.yaml de la configuration de votre robot MoveIt.
Exemple:
all:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.02
kinematics_solver_attempts: 1
mode: gd_c
Optimiseurs locaux actuellement disponibles :
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
Convention de dénomination : <solver type>_[<variant>_]<number of threads>
Remarques :
gd_* prennent en charge des types d'objectifs arbitraires.jac_* ne prennent en charge que les objectifs de pose mais pourraient en théorie être plus stables dans certains cas.gd_c ont généralement surpassé les autres solveurs locaux.gd_c , gd ou jac ).Vous pouvez forcer un optimiseur global à renvoyer un minimum local via la régularisation.
Pour le cas spécifique des mouvements incrémentiels du robot (c'est-à-dire jogging ), la solution la plus simple consiste à spécifier à la fois un PoseGoal et un RegularizationGoal spécial, qui tente de maintenir la solution IK de l'espace commun aussi proche que possible de la configuration donnée du robot. En règle générale, vous utiliserez un poids élevé pour le PoseGoal et un poids plus petit pour le régularisateur.
Vous pouvez également ajouter un MinimalDisplacementGoal au lieu du RegularizationGoal . Les deux objectifs tentent de maintenir la solution IK proche de l'état actuel du robot, mais diffèrent légèrement dans la gestion des articulations rapides et lentes du robot (par exemple, le PR2 a des articulations de bras rapides et une articulation de levage du torse plutôt lente). Vous voudrez peut-être jouer avec les deux objectifs pour voir lequel correspond le mieux à vos besoins.
Certains contrôleurs de robots industriels équipés de bras à 7 degrés de liberté se comportent comme s'ils travaillaient sur un bras à 6 degrés de liberté avec une articulation supplémentaire. Généralement, la valeur du joint supplémentaire peut être spécifiée, et une solution CI est ensuite recherchée pour les six joints restants. Ce comportement peut être obtenu dans bio_ik en combinant un PoseGoal pour l'effecteur final avec un JointPositionGoal pour l'une (n'importe laquelle) des articulations du robot.
Une autre astuce utile consiste à essayer de garder les articulations du robot centrées, car cela permettra aux mouvements (des articulations) du robot dans les deux sens. Combinez simplement le PoseGoal avec un CenterJointsGoal , et éventuellement également un RegularizationGaol .
Vous pouvez également combiner la régularisation avec un solveur gd_* local.
Le solveur bio_ik est basé sur un algorithme mémétique qui combine une optimisation basée sur le gradient avec une optimisation génétique et par essaim de particules.
En interne, les vecteurs de toutes les valeurs articulaires du robot sont utilisés pour coder différentes solutions intermédiaires (le génotype de l'algorithme génétique). Lors de l'optimisation, les valeurs des articulations sont toujours vérifiées par rapport aux limites actives inférieures et supérieures de l'articulation, afin que seules les configurations de robot valides soient générées.
Pour calculer la condition physique des individus, l’erreur cumulée sur tous les objectifs individuels donnés est calculée. Tout individu avec une erreur nulle est une solution exacte du problème CI, tandis que les individus avec une petite erreur correspondent à des solutions approximatives.
Les individus sont triés en fonction de leur forme physique et une optimisation basée sur le gradient est essayée sur les meilleures configurations, ce qui entraîne une convergence rapide et de bonnes performances pour de nombreux problèmes. Si aucune solution n’est trouvée grâce à l’optimisation basée sur le gradient, de nouveaux individus sont créés par un ensemble d’opérateurs de mutation et de recombinaison, ce qui entraîne une bonne exploration de l’espace de recherche.
Voir [3] et [4] pour plus de détails. Voir [5] et [6] pour une explication approfondie d'un algorithme évolutif antérieur pour l'animation de personnages de jeux vidéo.
Nous avons testé bio_ik sur de nombreux bras de robot différents, à la fois en utilisant l'API à effecteur unique traditionnelle et l'API à effecteur multiple avancée basée sur KinematicsQueryOptions.
Un autotest simple consiste à générer des configurations de robot valides aléatoires, à exécuter une cinématique avant pour calculer la pose de l'effecteur final résultant et à interroger le plugin IK pour trouver une configuration d'articulation de robot appropriée. Le succès est ensuite vérifié en exécutant à nouveau la cinématique avant et en vérifiant que la pose de l'effecteur final pour la solution IK générée correspond à la pose cible. Cette approche peut être exécutée facilement pour des milliers ou des millions de poses aléatoires, échantillonne tout l'espace de travail du robot et permet de générer rapidement des estimations du taux de réussite et du temps de solution pour le solveur IK sélectionné.
Bien entendu, l’exécution des tests nécessite d’installer les modèles de robots correspondants et ajoute de nombreuses dépendances. Par conséquent, ces tests ne sont pas inclus dans le package bio_ik standard, mais sont emballés séparément.
Pour plus de commodité, nous fournissons le package pr2_bioik_moveit , qui comprend également quelques démos bio_ik pour le robot de service PR2. Ce sont uniquement des démos de cinématique ; mais bien sûr, vous pouvez également essayer d'exécuter les démos sur le vrai robot (si vous en avez un) ou sur le simulateur Gazebo (si vous avez installé Gazebo).
Clonez simplement le package de description PR2 (à l'intérieur pr2_common ) et le package pr2_bioik_moveit dans votre espace de travail 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
Pour le test de performances FK-IK-FK, veuillez exécuter
roslaunch pr2_bioik_moveit env_pr2.launch
roslaunch pr2_bioik_moveit test_fk_ik.launch
... // wait for test completion and results summary
Cinématique et dynamique d'Orocos, http://www.orocos.org
P. Beeson et B. Ames, TRAC-IK : Une bibliothèque open source pour une résolution améliorée de la cinématique inverse générique , Actes de la conférence IEEE RAS Humanoids, Séoul, Corée, novembre 2015.
Philipp Ruppel, Norman Hendrich, Sebastian Starke, Jianwei Zhang, Cost Functions to Specify Full-Body Motion and Multi-Goal Manipulation Tasks , Conférence internationale de l'IEEE sur la robotique et l'automatisation (ICRA-2018), Brisbane, Australie. DOI : 10.1109/ICRA.2018.8460799
Philipp Ruppel, Optimisation des performances et mise en œuvre de la cinématique inverse évolutive dans ROS , mémoire de maîtrise, Université de Hambourg, 2017 PDF
Sebastian Starke, Norman Hendrich, Jianwei Zhang, Un algorithme évolutif mémétique pour le mouvement cinématique articulé en temps réel , IEEE Intl. Congrès sur le calcul évolutif (CEC-2017), p.2437-2479, 4-8 juin 2017, Saint-Sébastien, Espagne. DOI : 10.1109/CEC.2017.7969605
Sebastian Starke, Norman Hendrich, Dennis Krupke, Jianwei Zhang, Optimisation évolutive multi-objectifs pour la cinématique inverse sur les robots hautement articulés et humanoïdes , IEEE Intl. Conférence sur les robots et systèmes intelligents (IROS-2017), du 24 au 28 septembre 2017, Vancouver, Canada
