Im vorherigen Tutorial haben wir die Jointpositionen unseres Roboters gesteuert. Noch praktischer ist es aber wenn wir direkt den TCP (Tool Center Point) steuern können, d.h. die Spitze unseres Roboterarms an dem wir einen Greifer oder ein anderes Tool montiert haben mit dem wir unsere Umwelt manipulieren können. Wir wollen den TCP zu einer bestimmten Position im Raum befehligen, oder genauer gesagt, im Kartesischen Raum. Aber wie kommen wir von der Positionsregelung unserer Joints dazu, direkt die Position unseres TCP steuern zu können? Hier kommen die Konzept der Vorwärts- und Rückwärtskinematik ins Spiel in denen es darum geht, welche Jointpositionen zu welcher TCP Position führen und andersrum.
Um das Thema greifbar zu machen, schauen wir uns ein einfaches C++ Programm an das uns erlaubt, die Bewegung des TCP zu steuern. Wir implementieren eine ROS Anwendung welche die KDL-Bibliothek für die kinematischen Berechnungen nutzt und eine Schnittstelle zu unserem simulierten Roboter in Gazebo besitzt. Zuerst lassen wir das Programm laufen bevor wir dann Schritt für Schritt durch den Code gehen. Im letzten Abschnitt besprechen wir die Rückwärtskinematik (bzw. inverse Kinematik) im Detail und sehen uns deren Implementierung in der KDL-Bibliothek an. Also, anschnallen und los geht’s!
Klonen, bauen und ausführen des TCP-Positionsreglers
Aus dem vorherigen Tutorial über ros_control haben wir noch getunte Positionsregler für unsere UR5 Joints die wir in Gazebo simuliert haben. Falls du es noch nicht gelesen hast und es dich interessiert kannst du dir zuerst dieses Tutorial durchlesen. Ansonsten findest du das komplette Setup mit (einigermaßen) getunten Reglerparametern auch im Repository sodass du dich darum nicht zu kümmern brauchst.
Gehe zuerst sicher dass du Gazebo und ros_control, mit denen wir schon im vorherigen Tutorial gearbeitet haben, installiert hast. Falls nicht dann führe die folgenden Befehle in deinem Linux-Terminal aus:
$ curl -sSL http://get.gazebosim.org | sh
$ sudo apt-get install ros-$ROS_DISTRO-ros-control ros-$ROS_DISTRO-ros-controllersFalls du noch keinen Catkin Workspace für ROS aufgesetzt hast mach das bitte indem du den Instruktionen unter diesem Link folgst.
Zusätzlich brauchen wir das universal_robot package. Falls du eines der vorherigen Tutorials gemacht hast sollte das bereits vorhanden sein. Ansonsten, klone das Repository in den src Ordner in deinem Workspace und überprüfe die Abhängigkeit von anderen Packages:
~/catkin_ws/src$ git clone -b <distro>-devel https://github.com/ros-industrial/universal_robot.git
$ rosdep update
$ rosdep install --rosdistro $ROS_DISTRO --ignore-src --from-paths srcKlone außerdem die folgenden beiden Repositories in den src Ordner in deinem Workspace (Eventuell hast du das erste Repository schon wenn du das vorherige Tutorial über Jointpositionsregler gemacht hast):
~/catkin_ws/src$ git clone https://github.com/dairal/ur5-joint-position-control.git
~/catkin_ws/src$ git clone https://github.com/dairal/ur5-tcp-position-control.gitNun kannst du deinen Workspace bauen:
~/catkin_ws/src$ cd ..
~/catkin_ws$ catkin_make
~/catkin_ws$ source devel/setup.bashDamit bauen wir gleichzeitig auch unsere Anwendung für dieTCP-Steuerung. Die launch-Datei die wir nutzen um die Simulation zu laden startet Gazebo im pausierten Modus und lädt den Roboter mit einer bestimmten Startposition. Dazu – und um die Positionsregler für die Joints zu laden – führe den folgenden Befehl aus:
$ roslaunch ur5-joint-position-control ur5_gazebo_joint_position_control.launchZu Beginn nachdem alles gestarted ist liegt der UR5 Roboterarm am Boden und die Simulation ist pausiert. Wenn du auf den Play-Button in der unteren Leiste klickst, bewegt sich der Roboter zu der Startposition welche in der ur5_gazebo_pose.launch Datei definiert ist. Nun können wir den tcp-Positionsregler den wir vorher gebaut haben starten:
$ rosrun ur5-tcp-position-control tcp_position_controllerGroßartig, das Programm wird dir die aktuelle tcp-Position deines Roboters mitteilen und dich fragen, welche Distanz du in Richtung der einzelnen Koordinatenachsen (x,y,z) fahren möchtest. Starte am besten mit kleinen Werten wie 0.2 für die Bewegung, da die Einheit in Meter und der Arbeitsbereich deines Roboters begrenzt ist. Außerdem wird der kinematische Solver Probleme mit der Berechnung für bestimmte Zielpositionen habe. Zusätzlich fragt dich das Program wie lange (time) die Bewegung dauern soll (Wert in Sekunden).
Wie du sehen kannst ist die Performance der Regler noch nicht besonders gut, aber es funktioniert! Wenn du ein bisschen mit dem Program herumspielst wirst du feststellen, dass einige Zielpositionen besser funktionieren als andere und manche überhaupt nicht. Zum Teil spielt der Roboterarm sogar verrückt und kollabiert. In diesen Fällen solltest du alles noch einmal schließen und neu starten indem du die launch-Datei und die Anwendung neu lädst. Der Grund warum es für manche Zielpositionen nicht funkioniert können die kinematischen Berechnungen sein die sehr fragil sind, oder einfach dass die Zielposition vom Roboter nicht erreicht werden kann. Jetzt wollen wir und den Code für unsere Implementierung ansehen.
Der Code: Funktionaltät und Interface
Notiz: Die hier beschriebene Methode ist nur eine – relativ naive – Möglichkeit die TCP-Position eines Roboterarms zu steuern. Diese einfache Implementierung zielt darauf ab ein Verständnis für die zugrundeliegenden Konzepte von Vorwärts- und Rückwärtskinematik zu bekommen.
Im Folgenden gehe ich auf die wichtigsten Teile des Codes ein. Hier den ganzen Code zu zeigen würde den Rahmen dieses Artikels sprengen, allerdings ist der unwichtigere Rest zu Großteil repetitiver, sogennanter “boilder plate” code den zu besprechen nicht viel Mehrwert bringt. Du findest den kompletten Sourcecode in der Datei ur5_cartesian_position_controller.cpp in dem geklonten ur5-tcp-position-control.git Repository oder du kannst dir ihren Inhalt unter diesem Link ansehen.
Publishen und Subscriben
Ersteinmal möchten wir die aktuelle Position der Joints unseres Roboters wissen um daraus die TCP-Position zu berechnen von der aus wir starten. Wir wir schon im letzten Tutorial gesehen haben werden diese Werte von unseren Positionsreglern unter der Topic <name_des_joints>_position_controller/state/process_value gesendet (gepublisht). Wir “subscriben” also zu den Topics eines jeden einzelnen Joint-Positionsreglers um dies Werte zu erhalten. Der Folgende Codeschnipsel zeigt als Beispiel den Subscriber für den “shoulder pan joint”:
92 ros::Subscriber shoulder_pan_joint_sub = n.subscribe("/shoulder_pan_joint_position_controller/state", 1000, get_shoulder_pan_joint_position);In der callback-Funktion die immer aufgerufen wird wenn der Subscriber neue Daten erhält speichern wir die Werte in das jnt_pos_start Array.
18 const int Joints = 6;
19 KDL::JntArray jnt_pos_start(Joints);
20
21 void get_shoulder_pan_joint_position(const control_msgs::JointControllerState::ConstPtr& ctr_msg) {
22 jnt_pos_start(0) = ctr_msg->process_value;
23 }
Nach den Berechnungen wollen wir wiederum Positionsbefehle an alle Jointpositionsregler senden. Dafür erstellen wir sechs Publisher die die jeweilige topic <joint_name>_position_controller/command senden und speichern sie in dem joint_com_pub Array um später leichter darauf zugreifen zu können. Der folgende Code zeigt die Initialisierung des Arrays und die Zuweisung der /shoulder_pan_position_controller/command Topic zum ersten Publisher:
99 ros::Publisher joint_com_pub[6];
100 joint_com_pub[0] = n.advertise<std_msgs::Float64>("/shoulder_pan_joint_position_controller/command", 1000);Kinematsche Solver aus der KDL Bibliothek
Die KDL Bibliothek bietet ein Framework um kinematische Ketten zu modellieren und zu berechnen. Das ist exakt was wir brauchen um um unsere aktuelle TCP-Position basierend auf den Jointpositionen zu berechnen. Für diese Berechnungen muss KDL zuerst eine kinematische Kette unseres Roboters erstellen. In Zeile 109 generiert KDL einen sogenannten “Tree” (die Roboterstruktur) aus unserer URDF. Die URDF ist ein xml-Dokument das die Roboterstruktur beschreibt. Die URDF die wir für dieses Tutorial benutzen ist im Detail in den beiden vorigen Tutorials über Visualisierung und Simulation des UR5 Roboters beschreiben. Aus dieser “Tree”-Darstellung extrahiert KDL in Zeile 116 die kinematische Kette. Dafür müssen wir KDL sagen wo diese beginnt of wo sie endet. In unserem Fall reicht sie von der Basis bis zum allerletzten Link (wrist_3_link).
107 //Parse urdf model and generate KDL tree
108 KDL::Tree ur5_tree;
109 if (!kdl_parser::treeFromFile(urdf_path, ur5_tree)){
110 ROS_ERROR("Failed to construct kdl tree");
111 return false;
112 }
113
114 //Generate a kinematic chain from the robot base to its tcp
115 KDL::Chain ur5_chain;
116 ur5_tree.getChain("base_link", "wrist_3_link", ur5_chain);Als nächstes instanziieren wir die Solver welche die konkreten kinematischen Berechnungen durchführen. Wir haben einen für die Vorwärtskinematik der unsere TCP-Position basierend auf den Joint-Positionen berechnet und einen für die Rückwärtskinematik der die Joint-Positionen auf Grundlage der TCP-position berechnet. Der Solver der Rückwärtskinematik für die Joint-Positionen ik_solver benötigt zusätzlich als Argument einen Rückwärtskinematik-Solver für die Geschwindigkeit vel_ik_solver.
120 KDL::ChainIkSolverVel_pinv vel_ik_solver(ur5_chain, 0.0001, 1000);
121 KDL::ChainIkSolverPos_NR ik_solver(ur5_chain, fk_solver, vel_ik_solver, 1000);Wir werden uns am Ende des Artikels genauer ansehen wie die Forwärts- bzw. Rückwärtskinematik berechnet wird. Dann werde ich auch näher auf die mysteriösen Zahlen eingehen die wir als Parameter übergeben. Nachdem wir nun alles vorbereitet haben, kann das Programm mit der eigentlichen Arbeit beginnen.
Nun berechnen wir mit Hilfe unseres Solvers fk_solver für die Forwärtskinematik die TCP-Position. In Zeile 134 übergeben wir dem Solver die Joint-Positionen jnt_pos_start und einen leeren KDL::Frame tcp_pos_start in dem die resultierende TCP-Position gespeichert wird. Ein KDL::Frame is quasi ein Koordinatensystem welches durch Orientierung und die Position des Ursprungs definiert wird.
133 KDL::Frame tcp_pos_start;
134 fk_solver.JntToCart(jnt_pos_start, tcp_pos_start);
Nun kennen wir die TCP-Position von der wir starten, welche der Ursprung des berechneten KDL:Frame ist. Diese Information geben wir auf dem Bildschirm aus und fragen den Benutzer, wohin sich der TCP bewegt werden soll. Das geschieht innerhalb der get_goal_tcp_and_time() Funktion in Zeile 142. Außerdem fragen wir nach der gewünschten Dauer der Bewegung in Sekunden. Aus dem Input für x, y und z können wir den Ursprung des Koordinatensystems für die Zielposition vec_tcp_pos_goal berechnen indem wir die Werte einfach zu den aktuellen Koordinaten dazu addieren. Zusammen mit der neuen TCP-Position und der Orientierung tcp_pos_start.M die wir einfach von der Startpose übernehmen, können wir unseren neuen Ziel-Frame tcp_pos_goal in Zeile 144 konstruieren.
142 get_goal_tcp_and_time(tcp_pos_start, &vec_tcp_pos_goal, &t_max);
143
144 KDL::Frame tcp_pos_goal(tcp_pos_start.M, vec_tcp_pos_goal);Nun können wir in Zeile 148 unseren Sover für die Rückwärtskinematik aufrufen um die Joint-Positionen für unsere neue TCP-Position zu berechnen.
146 //Compute inverse kinematics
147 KDL::JntArray jnt_pos_goal(Joints);
148 ik_solver.CartToJnt(jnt_pos_start, tcp_pos_goal, jnt_pos_goal);Beachte, dass wir dem Solver auch die Anfangspositionen der Joints als Argument jnt_pos_start übergeben. Später werden wir die Gründe dafür diskutieren. Das Ergebnis wird in dem jnt_pos_goal Array gespeichert und das wars auch schon für die kinematischen Berechnungen!
Die Regelung der Joints
Jetzt müssen wir nur noch unsere Joints auf wohlerzogene Art zu ihrer neuen Position befehligen. Dazu benutzen wir einfach eine lineare Interpolation zwischen dem Startpunkt q_start und dem Zielpunkt q_goal innerhalb des definierten Zeitfensters t_max.
50 float compute_linear(double q_start, double q_goal, float t, float t_max) {
51 return((q_goal - q_start) * (t/t_max) + q_start);
52 }Jedes Mal wenn der while loop aufgerufen wird, berechnen wir den nächste Positionsbefehl für unsere Jointregler. Die Zeitschritte zwischen den Berechnungen werden von der Frequenz bestimmt mit der unser ROS-Node läuft. In diesem Fall ist die Frequenz 100 Hz, das heißt wir berechnen alle 10 Millisekunden einen neuen Positionsbefehl (Zeile 155). Wir schicken die neuen Werte an jeden Positionskontroller, warten 10 ms und berechnen dann den nächsten Zeitschritt, bis wir das ende unseres Zeitfensters t_max erreicht haben.
151 while(t<t_max) {
152 std_msgs::Float64 position[6];
153
154 for(int i=0; i<Joints; i++) {
155 position[i].data = compute_linear(jnt_pos_start(i), jnt_pos_goal(i), t, t_max);
156 joint_com_pub[i].publish(position[i]);
157 }
158
159 ros::spinOnce();
160 loop_rate.sleep();
161 ++count;
162 t += t_step;
163 } Das ist auch schon alles was wir brauchen um die TCP-Position unseres Roboters zu steuern. Unsere Methode hat dabei ein paar Nachteile. Während wir den Joint bewegen, lassen wir das Positionsfeedback von unseren Joints unberücksichtigt und am Ende halten wir einfach an ohne zu überprüfen ob die Joints ihre Zielposition erreicht haben. Damit vertrauen wir im Grunde genommen in die Fähigkeit unserer Positionsregler, der Positionsvorgabe sehr genau folgen zu können. Das funktioniert gut solange die Bewegungen relativ klein und nicht zu schnell werden. Für größere, schnellere Bewegungen kann es aber passieren, dass unsere Regler nicht mehr Schritt halten können, besonders wenn die Parameter nicht optimal getunt sind.
Abgesehen davon ist die lineare Berechnung unserer Positionsbefehle nicht die beste Lösung um echte Robotermotoren anzusteuern. Normalerweiße würde man dafür Polynome berechnen was einem erlaubt z.B. die Geschwindigkeiten am Anfang und am Ende der Bewegung definieren zu können. Alles in Allem aber haben wir ein einfaches Programm welches uns erlaubt die TCP-Position im kartesischen Raum zu steuern. Dabei können wir uns auf die Konzepte der Forwärts- und Rückwärtskinematik konzentrieren.
Wir werden uns jetzt kurz in diese beiden Themen vertiefen die sehr wichtige Grundlagen bilden um zu verstehen wie sich ein Roboter bewegt. Wir sehen uns außerdem an wie die KDL Bibliothek die nötigen Brechnungen für die Rückwärtskinematik implementiert. Keine Sorge, es ist nicht so komplex wie es auf den ersten Blick aussehen mag und wir werden die Dinge nicht überkompliziert machen.
Vorwärts-/Rückwärtskinematik in KDL
Wie können wir also die Jointpositionen berechnen wenn wir die TCP-Position im kartesischen Raum gegeben haben? Generell gibt es zwei Ansätze: Einen analytischen und einen numerischen. Wir diskutieren hier nur den numerischen Ansatz der von KDL und damit in diesem Tutorial benutzt wird.
In der numerischen Methode versuchen wir die Jointkonfiguration (die Winkel) Schritt für Schritt zu verschieben um uns der gewünschten TCP-Position anzunähern. Dafür versuchen wir eine eine Fehlerfunktion zu minimieren, in unserem Fall den Fehler zwischen unserer aktuellen und der gewünschten TCP-Position. Die TCP-Position (nennen wir sie \(X\)) besteht aus der Position \(x_{x/y/z}\) (dem Ursprung) und der Orientierung \(x_{Rx/Ry/Rz}\) und birgt insgesamt sechs Freiheitsgrade. Wenn wir einen Roboter mit \(n\) Joints haben ist jeder Freiheitsgrad abhängig von den Jointpositionen \(q_{1/…/n}\), d.h. ist eine Funktion der Jointpositionen (\(X = F(Q)\)).
Wir nehmen an dass \(X\) (eine \(6×1\) Matrix) die gewünschte TCP-Position und \(Q\) (eine \(nx1\) Matrix) unsere iterative Schätzung für die Jointpositionen ist. Außerdem definieren wir \(\Delta X\) als die Distanz zwischen der TCP-Position die sich aus diesen iterativen Schätzungen (über die Vorwärtskinematik) ergibt und unserer gewünschten TCP-Position. Damit können wir die Fehlerfunktion folgermaßen schreiben: \[\Delta X = X – F(Q)\] wobei \(F(Q)\) die TCP-Position für unsere aktuell geschätzten Jointpositionen ist. Wir wollen den Fehler minimieren, wenn wir also \(\Delta X\) auf Null setzen, d.h. \(0 = X – F(Q)\) bekommen wir die folgende Gleichung: \[X = F(Q)\] Die Funktion \(F()\) repräsentiert dabei unsere Forwärtskinematik die relativ einfach zu berechnen ist wie wir bereit kurz im Rviz-Tutorial besprochen haben. Unglücklicherweiße möchten wir \(Q\) basierend auf \(X\) wissen, d.h. die inverse (Rückwärtskinematik) welche nicht ganz so einfach zu berechnen ist. Daher müssen wir uns ein paar Tricks bedienen.
Wenn wir beide Seiten der Gleichung ableiten bekommen wir eine der fundamentalen Formeln der Robotik:
Diese Gleichung drückt die Abhängigkeit zwischen der TCP-Geschwindigkeit \(v\) im kartesischen Raum und den Jointgeschwindigkeiten (Winkelgeschwindigkeiten) \(\dot{q}\) aus. Die Jacobi-Matrix \(J\) hängt von der Roboterkinematik und den Jointwinkeln \(q\) in der aktuellen Konfiguration ab und bildet damit die Jointgeschwindigkeiten auf die TCP-Geschwindigkeit ab. Wir interessieren uns für die Jointpositionen die zu einer bestimmten TCP-Pose führen also teilen wir durch die Jacobi-Matrix und bringen sie auf die andere Seite der Gleichung: \[v * J(q)^{-1} = \dot{q} \]
Die inverse \(J(q)^{-1}\) einer Jacobi-Matrix kann nur direkt berechnet werden wenn es eine quadratische Matrix ist, d.h. die Anzahl der Freiheitsgrade muss mit der Anzahl der Joints übereinstimmen. Unglücklicherweise hat der UR5 Roboter sieben Joints also müssen wir eine Näherung für die inverse finden. Dies funktioniert z.B. mit der sogenannten pseudo-inversen Jacobi-Matrix (\(J(q)^+\)): \[v * J(q)^+ = \dot{q}\]
Und schon sind wir bei der zentralen Gleichung angelangt, die wir für die Berechnung der Rückwärtskinematik brauchen. Jetzt schauen wir uns das ganze in der KDL-Bibliothek an. Wie wir schon zuvor gesehen haben als wir den IK position Solver ik_solver (den Löser für unsere Rückwärtskinematik) konstruiert haben, nimmt KDL die berechnete kinematische Kette ur5_chain, einen Solver für die Vorwärtskinematik fk_solver, einen Rückwärtskinematik-Solver für die Geschwindigkeit vel_ik_solver und einen Integerwert für die maximale Anzahl an Iterationen als Argumente.
Der Solver für die Geschwindigkeit vel_ik_solver der davor konstruiert wird nimmt drei Argumente: Die kinematische Kette, einen Grenzwert für den Fehler \(\Delta X\) und die maximale Anzahl an Iterationen.
120 KDL::ChainIkSolverVel_pinv vel_ik_solver(ur5_chain, 0.0001, 1000);
121 KDL::ChainIkSolverPos_NR ik_solver(ur5_chain, fk_solver, vel_ik_solver, 1000);Schauen wir uns jetzt die ik_solver-Funktion CartToJnt innerhalb der KDL-Bibliothek an, die aus der kartesischen TCP-Position die Jointpositionen berechnet. Du kannst dir die Quelldatei auch hier ansehen.
42 int ChainIkSolverPos_NR::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
43 {
44 if (nj != chain.getNrOfJoints())
45 return (error = E_NOT_UP_TO_DATE);
46
47 if(q_init.rows() != nj || q_out.rows() != nj)
48 return (error = E_SIZE_MISMATCH);
49
50 q_out = q_init;
51
52 unsigned int i;
53 for(i=0;i<maxiter;i++){
54 if (E_NOERROR > fksolver.JntToCart(q_out,f) )
55 return (error = E_FKSOLVERPOS_FAILED);
56 delta_twist = diff(f,p_in);
57 const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
58 if (E_NOERROR > rc)
59 return (error = E_IKSOLVER_FAILED);
60 // we chose to continue if the child solver returned a positive
61 // "error", which may simply indicate a degraded solution
62 Add(q_out,delta_q,q_out);
63 if(Equal(delta_twist,Twist::Zero(),eps))
64 // converged, but possibly with a degraded solution
65 return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR);
66 }
67 return (error = E_MAX_ITERATIONS_EXCEEDED); // failed to converge
68 }
Nachdam das Array der Jointpositionen q_out mit den initialen Jointpositionen q_init initialisiert ist, wird die anfängliche TCP-Position in Zeile 54 mit dem Solver für die Vorwärtskinematik berechnet, was uns unser anfängliches \(F(Q)\) liefert. In Zeile 56 wird die Distanz zwischen der aktuellen TCP-Pose f und der Zielpose p_in über ein Zeitfenster von einer Sekunde differenziert. Damit bekommen wir eine TCP-Geschwindigkeit \(v\) (delta_twist) die zugleich auch unser \(\Delta X\) ist. Dann rufen wir die Funktion CartToJnt (Zeile 57) des Geschwindigkeits-Solvers vel_ik_solver auf. Wir übergeben die aktuelle Schätzung der Jointposition q_out (d.h. die Startposition q_init im Falle der ersten Iteration) und die Geschwindigkeit \(v\) und daraus wird die Jointgeschwindigkeit \(\dot{q}\) berechnet mit der Gleichung die wir uns vorher schon angesehen haben: \[ \dot{q} = J(q)^+ * v\] Das Ergebnis für \(\dot{q}\) wird in qdot_out gespeichert.
62 int ChainIkSolverVel_pinv::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
63 {
64 if (nj != chain.getNrOfJoints())
65 return (error = E_NOT_UP_TO_DATE);
66
67 if (nj != q_in.rows() || nj != qdot_out.rows())
68 return (error = E_SIZE_MISMATCH);
69
70 //Let the ChainJntToJacSolver calculate the jacobian "jac" for
71 //the current joint positions "q_in"
72 error = jnt2jac.JntToJac(q_in,jac);
73 if (error < E_NOERROR) return error;
74
75 double sum;
76 unsigned int i,j;
77
78 // Initialize near zero singular value counter
79 nrZeroSigmas = 0 ;
80
81 //Do a singular value decomposition of "jac" with maximum
82 //iterations "maxiter", put the results in "U", "S" and "V"
83 //jac = U*S*Vt
84 svdResult = svd.calculate(jac,U,S,V,maxiter);
85 if (0 != svdResult)
86 {
87 qdot_out.data.setZero();
88 return (error = E_SVD_FAILED);
89 }
90
91 // We have to calculate qdot_out = jac_pinv*v_in
92 // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
93 // qdot_out = V*S_pinv*Ut*v_in
94
95 //first we calculate Ut*v_in
96 for (i=0;i<jac.columns();i++) {
97 sum = 0.0;
98 for (j=0;j<jac.rows();j++) {
99 sum+= U[j](i)*v_in(j);
100 }
101 //If the singular value is too small (<eps), don't invert it but
102 //set the inverted singular value to zero (truncated svd)
103 if ( fabs(S(i))<eps ) {
104 tmp(i) = 0.0 ;
105 // Count number of singular values near zero
106 ++nrZeroSigmas ;
107 }
108 else {
109 tmp(i) = sum/S(i) ;
110 }
111 }
112 //tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
113 //it with V to get qdot_out
114 for (i=0;i<jac.columns();i++) {
115 sum = 0.0;
116 for (j=0;j<jac.columns();j++) {
117 sum+=V[i](j)*tmp(j);
118 }
119 //Put the result in qdot_out
120 qdot_out(i)=sum;
121 }
122
123 // Note if the solution is singular, i.e. if number of near zero
124 // singular values is greater than the full rank of jac
125 if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
126 return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular
127 } else {
128 return (error = E_NOERROR); // have converged
129 }
130 }
Die Jacobi-Matrix \(J(q)\) wird in Abhängigkeit von den Jointpositionen in Zeile 72 brechnet. Danach wird die pseudo-inverse \(J(q)^+\) mit der Methode der Singulärwertzerlegung berechnet, wobei an die zugehörige Funktion die maximale Anzahl an Iterationen als Argument übergeben wird. Die Zerlegung sieht dann so aus: \[J(q)^+ = V * S_{pinv} * U_t \] Nach der kompletten Berechnung wird das sich ergebende \(\dot{q}\) in Zeile 120 in dem Array qdot_out gespeichert. Wenn das Program zum Positions-Solver ik_solver zurückgekehrt ist werden die Jointgeschwindigkeiten (\dot{q}) integriert da wir ja am Ende die Jointposition \(q\) wissen möchten. Die Integration wird in der CartToJnt Funktion des Positions-Solvers in Zeile 62 durchgeführt indem \(\dot{q}\) einfach zur vorherigen Schätzung der Jointpositionen q_out hinzugefügt wird. Danach beginnt die nächste Iteration mit diesem neuen Wert. Das geht so lange bis die TCP-Geschwindigkeit \(v\) (d.h. \(\Delta X\)) zwischen aktueller TCP-Position für die geschätzte Jointkonfiguration und der gewünschten Zielposition des TCP unter einer bestimmten Grenze liegt. Dann ist die aktuelle Pose für unsere Zwecke nah genug an der gewünschten Pose.
Das wars! So wird die Rückwärtskinematik in der KDL-Bibliothek berechnet. Ich hoffe das war ein weinig hilfreich und ihr konntet der Beschreibung einigermaßen folgen. Wir haben uns natürlich nicht jedes Detail angesehn aber wenn du mehr wissen willst z.B. wie die Singulärwertzerlegung für \(J(q)^+\) funktioniert dann wird dir eine Googlesuche viele Ergebnisse liefern. Es gibt noch andere Arten die Rückwärtskinematik zu berechen wie zum Beispiel den recht populären ikfast-Algorithmus, aber dazu kommen wir ein ander Mal. Danke dass du bis zum Schluss des Artikels dabei geblieben bist und ich werde bald mit weitern Tutorials zurück sein!
