Robótica: Cinemática Inversa (Inverse Kinematics) en C++ con Orocos

Anteriormente en este blog ya se ha tratado brevemente el concepto  de la cinemática directa  (fordward kinematics  o FK) : “Fordward Kinematics with Python”. Hoy abordaremos otro concepto muy importante en robots manipuladores: la cinemática inversa (Inverse Kinematics o IK) que básicamente es el problema complementario. Recordemos la cinemática directa (FK) con un ejemplo: dado un robot con con tres articulaciones (a,b,c) y una forma física conocida (pose relativa entre articulaciones, etc) ¿donde estará el actuador final (típicamente una garra) si establecemos los valores de los ángulos de las articulaciones a (a=a0,b=b0,c=c0)?

Por el contrario la cinemática inversa (IK) presenta el enunciado complementario: como posicionar las articulaciones para lograr que el actuador se ubique en una pose concreta. El objetivo es lograr colocar todas las articulaciones del brazo robótico en alguna configucación de forma que el actuador final quede en una pose objetivo determinada (posición y ángulo) . Entonces las IK responde a la pregunta de: ¿es posible encontrar una configuración  (a0,b0,c0) tal que el actuador final esté en la pose deseada?

Como se soluciona el problema de la cinemática inversa. Un problema de cinemática inversa puede no tener solución, tenerla, o tener muchas (en robots redundantes). En cualquier caso la búsqueda de la solución suele realizarse mediante métodos de búsquedas de raíces numéricos. ie: método de Newton-Rapson. Esto puede resultar en cálculos lentos, por lo que habitualmente en una implementación real se acota el tiempo máximo (o iteraciones) que debe realizar el algorimo de búsqueda. Por último, para robots con pocos grados de libertad, existen soluciones analíticas. Algunas herramientas como OpenRave utiliza un método denominado ikfast donde realiza una generación de código específico para una caracterización cinemática determinada. Esta técnica le permite tiempos de respuestas mucho mas rápidos. En cualquier caso, en este post utilizaremos la herramientas Orocos y su paquete KDL.

Orocos es un RSFs (Robotic Software Framework) que fué impulsado por el EURON a principios de la década con el objetivo de unificar la metodología y fomentar la estandarización en el desarrollo de arquitecturas software para sistemas robóticos. Este proyecto está centrado en la creación de sistemas robóticos en entornos industriales y está compuesto por cuatro módulos principales: real time toolkit (rtt), orocos component library (ocl), bayesian filtering library (bcl) y kinematics dynamic library (kdl). KDL y BFL son librerias bastante independientes y están también integradas en otros RSFs como ROS: ros-kdl y ros-bfl

KDL el el componente que nos interesa para este post. Se trata de una librería que se puede instalar independientemente del resto de oOocos y  permite definir robots manipuladores y realizar el cálculo de FK y IK. A continuación se muestra el proceso básico de instalación de orocos-kdl en ubuntu y posteriormente un ejemplo de uso de la IK:

~$ sudo apt-get install libeigen2-dev
~$ svn checkout http://svn.mech.kuleuven.be/repos/orocos/trunk/kdl
~$ cd kdl
~/kdl$ ccmake
~/kdl$ make
~/kdl$ make install

Ahora, dado el código c++ que se muestra abajo, el proceso de compilación sería:

~/kdl-sample $ g++ `pkg-config –cflags orocos-kdl` `pkg-config –libs orocos-kdl` kdl-sample.cpp -o executable-kdl-sample
~/kdl-sample $ export LD_LIBRARY_PATH=/usr/local/lib/
~/kdl-sample $ ./executable-kdl-sample

En el siguiente código se modela un robot de 6 grados de libertad. En concreto el robot es el dibujado en la figura 1.

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include <iostream>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/frames.hpp>

using namespace KDL;

int main ()
{
 //Se define la caracterización cinemática del robot:
 KDL::Chain chain;
 chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1))));
 chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,1))));
 chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,1))));
 chain.addSegment(Segment(Joint(Joint::RotZ)));
 chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,1))));
 chain.addSegment(Segment(Joint(Joint::RotZ)));

 //se crean los solvers de cinemática directa y cinemática inversa:
 ChainFkSolverPos_recursive fksolver(chain);//objeto para calcular la cinemática directa del robot
 ChainIkSolverVel_pinv iksolver1v(chain);//objeto para calcular la cinemática inversa
 ChainIkSolverPos_NR iksolver1(chain,fksolver,iksolver1v,100,1e-6);//como máximo 100 iteraciones, parar si el error es menor que 1e-6

 //se crean dos tuplas de configuración q_init (configuración inicial) y q (configuración resultante)
 JntArray q(chain.getNrOfJoints());
 JntArray q_init(chain.getNrOfJoints());

 //Pose objetivo del actuador final (end-effector)
 Frame F_dest;
 F_dest.p= Vector(1,1,1);
 F_dest.M= Rotation(1,0,0,
0,1,0,
0,0,1);

//aplicación de la cinemática inversa
 int ret = iksolver1.CartToJnt(q_init,F_dest,q);

//se imprimen por pantalla los resultados
 printf("ret= %d, q(0)=%f,q(1)=%f,q(2)=%f,q(3)=%f,q(4)=%f,q(5)=%f\n",ret, q(0), q(1), q(2), q(3), q(4), q(5));
 KDL::Frame cartpos;

 // Para chequear la validez del cálculo comprobamos la cinemática directa de la configuración q obtenida
 bool kinematics_status;
 kinematics_status = fksolver.JntToCart(q,cartpos);
 if(kinematics_status>=0){
 std::cout << cartpos <<std::endl;
 printf("%s \n","Succes, thanks KDL!");
 }else{
 printf("%s \n","Error: could not calculate forward kinematics : (");
 }
}

La variable ret == 0 indica que la solución se ha encontrado para la configuración:

ret= 0,
q(0)=-0.785398,
q(1)=-2.709875,
q(2)=1.047198,
q(3)=0.000000,
q(4)=1.662677,
q(5)=0.785398

La comprobación de la cinemática directa nos da el siguiente resultado:

[[           1, 8.39999e-15, 2.10839e-15;
-8.39999e-15,           1, 1.61959e-16;
-2.06065e-15,-1.62154e-16,           1]

[           1,           1,           1]]

Asumiendo que hay números muy bajos con valor despreciable se puede ver mas claramente que el resultado es:

[[ 1, 0, 0;
0, 1,0;
0,0,1]

[  1,1,1]]

Es decir, La cinemática inversa con KDL ha funcionado correctamente porque ha logrado posicionar el actuador final en la pose que nosotros queríamos.

Succes, thanks KDL!

2 Responses to Robótica: Cinemática Inversa (Inverse Kinematics) en C++ con Orocos

  1. nilton dice:

    Hola Pablo, como estas.
    Felicitarte por tu blog francamente es una gran ayuda, y bueno tengo una pregunta con respecto a la libreria KDL que utilizas, sabes como compilar en visual estudio, he tratado de hacerlo pero no encuentro un file que se llama Eigen/Core en el source de la libreria. Tendras un manual? o puedes hacer un block sobre eso, disculpame por ser novato.
    Saludos

  2. Pablo dice:

    Te hace falta las librerías de Eigen, no están en el sistema por lo que tendrás que bajarte las librerías de la web oficial. Recuerda añadir a las propiedades del proyectos los directorios de los ficheros de inclusión y los directorios de las librerías, así como añadir a las propiedades del linker las librerías a linkar.

    Vamos esto te lo digo a ojo porque yo no uso Windows sino linux y el proceso es ligeramente distinto. Suerte y por favor, cualquier opinión sobre la librería o temas de cinemática compártelos con nosotros!

    Saludos.

Responder

Introduce tus datos o haz clic en un icono para iniciar sesión:

Logo de WordPress.com

Estás comentando usando tu cuenta de WordPress.com. Cerrar sesión / Cambiar )

Imagen de Twitter

Estás comentando usando tu cuenta de Twitter. Cerrar sesión / Cambiar )

Foto de Facebook

Estás comentando usando tu cuenta de Facebook. Cerrar sesión / Cambiar )

Google+ photo

Estás comentando usando tu cuenta de Google+. Cerrar sesión / Cambiar )

Conectando a %s

A %d blogueros les gusta esto: