bio_ik
1.0.0
Dieses Repository bietet eine BSD-lizenzierte eigenständige Implementierung verschiedener Optimierungsmethoden zur effizienten Lösung verallgemeinerter inverser Kinematikprobleme.
Das gesamte Modul wurde von Philipp Ruppel im Rahmen seiner Masterarbeit umgesetzt.
Für eine C++-basierte Neuimplementierung des ursprünglichen „BioIK“-Algorithmus, wie er ursprünglich im Unity-Store verkauft wurde, können Sie den nicht standardmäßigen Modus bio1 verwenden. Der Standardmodus bio2_memetic teilt keinen Code mit dieser Implementierung, übertrifft diese nachweislich in Bezug auf Erfolgsrate, Präzision und Effizienz und ist tatsächlich für präzise Roboteranwendungen verwendbar [4].
Sie benötigen die ROS-Version Indigo oder neuer (wiki.ros.org). Die Software wurde auf Ubuntu Linux 16.04 LTS mit ROS Kinetic entwickelt, wurde aber auch auf Ubuntu Linux 14.04 LTS mit ROS Indigo getestet. Neuere Versionen von ROS sollten funktionieren, erfordern jedoch möglicherweise einige Anpassungen. Nachfolgend finden Sie versionspezifische Anweisungen.
Laden Sie das bio_ik Paket herunter und entpacken Sie es in Ihren Catkin-Arbeitsbereich.
Führen Sie catkin_make aus, um Ihren Arbeitsbereich zu kompilieren:
roscd
cd src
git clone https://github.com/TAMS-Group/bio_ik.git
roscd
catkin_make
Konfigurieren Sie Moveit so, dass bio_ik als Kinematiklöser verwendet wird (siehe nächster Abschnitt).
Verwenden Sie Moveit, um Bewegungen zu planen und auszuführen, oder verwenden Sie Ihren eigenen Code zusammen mit dem Knoten move_group um Ihren Roboter zu bewegen.
Wie üblich wird die öffentliche API in den öffentlichen Header-Dateien für das Paket bio_ik angegeben, die sich im Unterverzeichnis include/bio_ik befinden; Die Quellen einschließlich einiger privater Header-Dateien befinden sich im Unterverzeichnis src .
Zur Benutzerfreundlichkeit und Kompatibilität mit vorhandenem Code ist der bio_ik-Algorithmus als Moveit-Kinematik-Plugin gekapselt. Daher kann bio_ik als direkter Ersatz des standardmäßigen Orocos/KDL-basierten IK-Lösers verwendet werden. Anhand des Namens eines Endeffektors und einer 6-DOF-Zielhaltung sucht bio_ik nach einer gültigen Robotergelenkkonfiguration, die das angegebene Ziel erreicht.
In unseren Tests (siehe unten) übertraf bio_ik sowohl hinsichtlich der Erfolgsquote als auch der Lösungszeit regelmäßig den Solver von Orocos [1] und ist mit trac-ik [2] konkurrenzfähig. Der bio_ik-Algorithmus kann auch für Systeme mit hohem DOF wie Roboterschlangen verwendet werden und konvergiert automatisch zu den besten Näherungslösungen für Arme mit niedrigem DOF, bei denen einige Zielposen nicht genau erreichbar sind.
Während Sie die Moveit-Konfigurationsdateien von Hand schreiben können, ist es am einfachsten, den Moveit-Einrichtungsassistenten für Ihren Roboter auszuführen und dann bei der Konfiguration der Endeffektoren bio_ik als IK-Solver auszuwählen. Nach der Konfiguration kann der Solver über die Standard-Moveit-API aufgerufen oder interaktiv von rviz aus mithilfe des MotionPlanning-GUI-Plugins verwendet werden.
Stellen Sie sicher, dass Sie über ein URDF- (oder Xacro-)Modell für Ihren Roboter verfügen.
Führen Sie den Moveit-Setup-Assistenten aus, um die Moveit-Konfigurationsdateien zu erstellen:
rosrun moveit_setup_assistant moveit_setup_assistant
Der Einrichtungsassistent sucht automatisch nach allen verfügbaren IK-Solver-Plugins in Ihrem Arbeitsbereich. Daher können Sie einfach „bio_ik“ als IK-Löser aus der Dropdown-Liste für jeden Endeffektor auswählen und dann die Kinematikparameter konfigurieren, nämlich die Standardpositionsgenauigkeit (Meter) und das Timeout (in Sekunden). Für typische 6-DOF- oder 7-DOF-Arme sollten eine Genauigkeit von 0,001 m (oder weniger) und ein Timeout von 1 ms in Ordnung sein. Komplexere Roboter benötigen möglicherweise eine längere Auszeit.
Generieren Sie die Moveit-Konfigurationsdateien über den Setup-Assistenten. Natürlich können Sie die Konfigurationsdatei config/kinematics.yaml auch mit Ihrem bevorzugten Texteditor bearbeiten. Eine Konfiguration für den PR2-Roboter könnte beispielsweise so aussehen:
# 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
Führen Sie für einen ersten Test den von Moveit erstellten Demo-Start aus. Sobald rviz ausgeführt wird, aktivieren Sie das Bewegungsplanungs-Plugin und wählen Sie dann einen der Endeffektoren Ihres Roboters aus. Rviz sollte eine interaktive 6-D-Markierung (Position und Ausrichtung) für den/die ausgewählten Endeffektor(e) anzeigen. Bewegen Sie den interaktiven Marker und beobachten Sie, wie bio_ik Posen für Ihren Roboter berechnet.
Wenn Sie auch die bio_ik-Demo installiert haben (siehe unten), sollten Sie in der Lage sein, eine der vordefinierten Demos auszuführen:
roslaunch pr2_bioik_moveit demo.launch
roslaunch pr2_bioik_moveit valve.launch
roslaunch pr2_bioik_moveit dance.launch
Sie können bio_ik jetzt aus Ihren C/C++- und Python-Programmen verwenden und dabei die Standard-Moveit-API verwenden. So fordern Sie explizit eine IK-Lösung in C++ an:
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
);
Für viele Roboteranwendungen ist es wichtig, mehr als nur eine einzelne Endeffektor-Pose anzugeben. Typische Beispiele sind:
In bio_ik werden solche Aufgaben als Kombination mehrerer Einzelziele spezifiziert.
Der Algorithmus versucht dann, eine Roboterkonfiguration zu finden, die alle vorgegebenen Ziele gleichzeitig erfüllt, indem er eine quadratische Fehlerfunktion minimiert, die aus den gewichteten Einzelzielen gebildet wird. Während die aktuelle Moveit-API Aufgaben mit mehreren Zielen nicht direkt unterstützt, stellt sie die KinematicQueryOptions-Klasse bereit. Daher stellt bio_ik einfach einen Satz vordefinierter Bewegungsziele bereit und eine Kombination der benutzerdefinierten Ziele wird über Moveit an den IK-Solver übergeben. In Moveit sind keine API-Änderungen erforderlich, aber die Verwendung des IK-Lösers besteht nun darin, die gewichteten Ziele über die KinematicQueryOptions zu übergeben. Zu den vordefinierten Zielen gehören:
Um ein Bewegungsproblem an Ihrem Roboter zu lösen, besteht die Kunst nun darin, eine geeignete Kombination einzelner Ziele zu konstruieren.
Im folgenden Beispiel wollen wir mit den linken und rechten Greifern des PR2-Roboters ein Ventilrad greifen und dann langsam drehen :
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);
Wir haben uns auch ein paar sekundäre Ziele gesetzt. Zunächst möchten wir, dass der Kopf des PR2 auf die Mitte des Ventils zeigt. Zweitens wollen wir Gelenkbegrenzungen möglichst an allen Gelenken vermeiden. Drittens möchten wir, dass IK-Lösungen so nah wie möglich an der vorherigen Gelenkkonfiguration sind, was kleine und effiziente Bewegungen bedeutet. Dies wird durch Hinzufügen des MinimalDisplacementGoal gehandhabt. Viertens wollen wir Rumpfhebebewegungen vermeiden, die beim PR2 sehr langsam sind. All dies lässt sich leicht spezifizieren:
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);
Für die eigentliche Drehbewegung berechnen wir in einer Schleife einen Satz erforderlicher Greiferpositionen:
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
}
Wenn Sie den Code ausführen, greift der PR2 nach dem Ventilrad und dreht es. Hin und wieder kann es mit der aktuellen Armkonfiguration das Ventil nicht erreichen und greift das Rad erneut.
Weitere Beispiele finden Sie unter [3] und [4].
BioIK wurde entwickelt, um effizient gute Lösungen für nichtkonvexe inverse Kinematikprobleme mit mehreren Zielen und lokalen Minima zu finden. Bei einigen Anwendungen kann dies jedoch zu nicht intuitiven Ergebnissen führen. Wenn es für ein bestimmtes IK-Problem mehrere mögliche Lösungen gibt und der Benutzer nicht explizit angegeben hat, welche er wählen soll, kann ein Ergebnis zufällig aus der Menge aller gültigen Lösungen ausgewählt werden. Beim inkrementellen Verfolgen eines kartesischen Pfads kann es zu unerwünschten Sprüngen, Verwacklungen usw. kommen. Um mithilfe von BioIK schrittweise eine glatte Trajektorie zu erzeugen, sollte das gewünschte Verhalten explizit angegeben werden, was auf zwei Arten erfolgen kann.
BioIK bietet eine Reihe verschiedener Löser, darunter globale Optimierer und lokale Optimierer. Standardmäßig verwendet BioIK einen memetischen globalen Optimierer ( bio2_memetic ). Eine andere Solver-Klasse kann ausgewählt werden, indem Sie den mode in der Datei kinematics.yaml Ihrer MoveIt-Roboterkonfiguration festlegen.
Beispiel:
all:
kinematics_solver: bio_ik/BioIKKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.02
kinematics_solver_attempts: 1
mode: gd_c
Derzeit verfügbare lokale Optimierer:
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
Namenskonvention: <solver type>_[<variant>_]<number of threads>
Hinweise:
gd_* -Löser unterstützen beliebige Zieltypen.jac_* -Löser unterstützen nur Posenziele, könnten aber theoretisch in einigen Fällen stabiler sein.gd_c -Varianten normalerweise die anderen lokalen Löser.gd_c , gd oder jac ).Sie können einen globalen Optimierer durch Regularisierung zwingen, ein lokales Minimum zurückzugeben.
Für den speziellen Fall inkrementeller Roboterbewegungen (auch Jogging genannt) besteht die einfachste Lösung darin, sowohl ein PoseGoal als auch das spezielle RegularizationGoal anzugeben, das versucht, die Gelenkraum-IK-Lösung so nah wie möglich an der gegebenen Roboter-Seed-Konfiguration zu halten. Normalerweise würden Sie ein hohes Gewicht für PoseGoal und ein kleineres Gewicht für den Regularizer verwenden.
Sie können anstelle des RegularizationGoal auch ein MinimalDisplacementGoal hinzufügen. Beide Ziele versuchen, die IK-Lösung nahe am aktuellen/säenden Roboterzustand zu halten, unterscheiden sich jedoch geringfügig in der Handhabung schneller und langsamer Robotergelenke (z. B. verfügt der PR2 über schnelle Armgelenke und ein eher langsames Torso-Hebegelenk). Vielleicht möchten Sie mit beiden Zielen experimentieren, um herauszufinden, welches Ihren Anforderungen besser entspricht.
Einige Industrierobotersteuerungen an 7-DOF-Armen verhalten sich so, als würden sie an einem 6-DOF-Arm mit einem zusätzlichen Gelenk arbeiten. Normalerweise kann der Wert des zusätzlichen Gelenks angegeben werden, und dann wird eine IK-Lösung für die verbleibenden sechs Gelenke gesucht. Dieses Verhalten kann in bio_ik erreicht werden, indem ein PoseGoal für den Endeffektor mit einem JointPositionGoal für eines (beliebiges) der Robotergelenke kombiniert wird.
Ein weiterer nützlicher Trick besteht darin, die Gelenke des Roboters zentriert zu halten, da dies Bewegungen des Roboters (Gelenks) in beide Richtungen ermöglicht. Kombinieren Sie einfach das PoseGoal mit einem CenterJointsGoal und optional auch einem RegularizationGaol .
Sie können die Regularisierung auch mit einem lokalen gd_* Löser kombinieren.
Der bio_ik-Löser basiert auf einem memetischen Algorithmus, der gradientenbasierte Optimierung mit genetischer und Partikelschwarmoptimierung kombiniert.
Intern werden Vektoren aller Robotergelenkwerte verwendet, um verschiedene Zwischenlösungen (den Genotyp des genetischen Algorithmus) zu kodieren. Bei der Optimierung werden die Gelenkwerte immer mit den aktiven unteren und oberen Gelenkgrenzen verglichen, sodass nur gültige Roboterkonfigurationen generiert werden.
Um die Fitness von Einzelpersonen zu berechnen, wird der kumulative Fehler über alle vorgegebenen individuellen Ziele berechnet. Jede Person mit einem Fehler von Null ist eine exakte Lösung für das IK-Problem, während Personen mit einem kleinen Fehler einer Näherungslösung entsprechen.
Die einzelnen Personen werden nach ihrer Fitness sortiert, und bei den besten wenigen Konfigurationen wird eine verlaufsbasierte Optimierung ausprobiert, was bei vielen Problemen zu schneller Konvergenz und guter Leistung führt. Wenn durch die gradientenbasierte Optimierung keine Lösung gefunden wird, werden durch eine Reihe von Mutations- und Rekombinationsoperatoren neue Individuen erstellt, was zu einer guten Suchraumerkundung führt.
Weitere Einzelheiten finden Sie unter [3] und [4]. In [5] und [6] finden Sie eine ausführliche Erläuterung eines früheren evolutionären Algorithmus zur Animation von Videospielcharakteren.
Wir haben bio_ik an vielen verschiedenen Roboterarmen getestet, sowohl mit der traditionellen Single-End-Effektor-API als auch mit der erweiterten Multi-End-Effektor-API basierend auf KinematicsQueryOptions.
Ein einfacher Selbsttest besteht darin, zufällige gültige Roboterkonfigurationen zu generieren, die Vorwärtskinematik zur Berechnung der resultierenden Endeffektorhaltung auszuführen und das IK-Plugin abzufragen, um eine geeignete Robotergelenkkonfiguration zu finden. Anschließend wird der Erfolg überprüft, indem die Vorwärtskinematik erneut ausgeführt und überprüft wird, ob die Endeffektor-Pose für die generierte IK-Lösung mit der Zielpose übereinstimmt. Dieser Ansatz kann problemlos für Tausende oder Millionen zufälliger Posen ausgeführt werden, erfasst den gesamten Arbeitsbereich des Roboters und ermöglicht die schnelle Erstellung von Erfolgsraten- und Lösungszeitschätzungen für den ausgewählten IK-Löser.
Natürlich erfordert die Durchführung der Tests die Installation der entsprechenden Robotermodelle und bringt viele Abhängigkeiten mit sich. Daher sind diese Tests nicht im Standardpaket von bio_ik enthalten, sondern werden separat verpackt.
Der Einfachheit halber stellen wir das Paket pr2_bioik_moveit zur Verfügung, das auch einige bio_ik-Demos für den PR2-Serviceroboter enthält. Dies sind reine Kinematik-Demos; Sie können aber natürlich auch versuchen, die Demos auf dem echten Roboter (falls Sie einen haben) oder dem Gazebo-Simulator (falls Sie Gazebo installiert haben) auszuführen.
Klonen Sie einfach das PR2-Beschreibungspaket (in pr2_common ) und das pr2_bioik_moveit Paket in Ihren Catkin-Arbeitsbereich:
roscd
cd src
git clone https://github.com/PR2/pr2_common.git
git clone https://github.com/TAMS-Group/bioik_pr2.git
catkin_make
Führen Sie den FK-IK-FK-Leistungstest bitte aus
roslaunch pr2_bioik_moveit env_pr2.launch
roslaunch pr2_bioik_moveit test_fk_ik.launch
... // wait for test completion and results summary
Orocos Kinematik und Dynamik, http://www.orocos.org
P. Beeson und B. Ames, TRAC-IK: Eine Open-Source-Bibliothek zur verbesserten Lösung generischer inverser Kinematik , Proceedings of the IEEE RAS Humanoids Conference, Seoul, Korea, November 2015.
Philipp Ruppel, Norman Hendrich, Sebastian Starke, Jianwei Zhang, Cost Functions to Specify Full-Body Motion and Multi-Goal Manipulation Tasks , IEEE International Conference on Robotics and Automation, (ICRA-2018), Brisbane, Australien. DOI: 10.1109/ICRA.2018.8460799
Philipp Ruppel, Leistungsoptimierung und Implementierung evolutionärer inverser Kinematik in ROS , MSc-Arbeit, Universität Hamburg, 2017 PDF
Sebastian Starke, Norman Hendrich, Jianwei Zhang, A Memetic Evolutionary Algorithm for Real-Time Articulated Kinematic Motion , IEEE Intl. Congress on Evolutionary Computation (CEC-2017), S. 2437-2479, 4.–8. Juni 2017, San Sebastian, Spanien. DOI: 10.1109/CEC.2017.7969605
Sebastian Starke, Norman Hendrich, Dennis Krupke, Jianwei Zhang, Multi-Objective Evolutionary Optimization for Inverse Kinematics on Highly Articulated and Humanoid Robots , IEEE Intl. Konferenz über intelligente Roboter und Systeme (IROS-2017), 24.–28. September 2017, Vancouver, Kanada
