Adaptar el fichero .m de generación de trayectoria:

[irivas-uma]

• [x] Para mover el robot tendrás que mandarle una posición y una orientación, y en el nodo del robot se usa la representación angle-axis. Por tanto, a tu fichero de generación de trayectoria tienes que añadirle la orientación del robot, que será perpendicular al plano de la superficie, y tendrá que ir dado en esa representación. Así, la salida de tu función será una trayectoria formada por 6 parámetros: las coordenadas x,y,z de cada punto y la orientació.

• [x] Guarda la trayectoria generada en un fichero .mat, que será el fichero que utilizaremos para mandarle al robot la trayectoria.

[mariajesusmoragarcia]

Una pregunta Irene, ¿la orientación sería la misma que la del parámetro de entrada en ángulos de euler?

[irivas-uma]

No, la orientación que acepta el robot en el topic de entrada está dada en forma de vector de rotación (https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/explanation-on-robot-orientation/). Tendrás que hacer la transformación de ángulos de Euler a vector de rotación. En Matlab hay funciones que lo hacen.

[mariajesusmoragarcia]

No lo entiendo, entonces la salida de la función no puede ser 6 parámetros, no? Para el cálculo de las posiciones si he utilizado la matriz de rotación, pero a la hora de pasar el array con la trayectoria generada había pensado en mandar las posiciones x,y,z y la orientación alpha, beta, gamma, directamente del vector de Euler de entrada, no se si me he explicado bien. Le adjunto el último cambio:

function trayectoria = generador_trayectoria(p0, r1, r2, eul)
    close all;

    % Nos aseguramos que la pose p0 sean 1x3
    if size(p0,1) ~= 3
         error('p0 is not 3x1 vector');
    end

    % Nos aseguramos que el vector de orientación sea 1x3
    if size(eul,1) ~= 1
        error('eul is not 1x3 vector');
    end   

    % Matriz de orientación
    rotmZYX  = eul2rotm(eul);

    % Punto inicial
    x0 = p0(1);
    y0 = p0(2);
    z0 = p0(3);
    pini = rotmZYX*[x0; y0; z0];
    plot3(pini(1), pini(2), pini(3),'mO');
    hold on;

    % Sistema de referencia
    T = [[rotmZYX, [0;0;0]]; [0 0 0 1]];
    createFRAME(T,'m','To',2);

    t = 0:pi/200:2*pi;

    % Calculamos el centro de la elipse
    xC = x0 - r1*cos(0);
    yC = y0 - r2*sin(0);
    zC = 0;

    C = rotmZYX*[xC; yC; zC];
    plot3(C(1), C(2), C(3), 'b*')

    % Calculamos el array de puntos de la elipse sin orientacion
    for i = 1:length(t)
        x_(i) = xC - r1*cos(i);
        y_(i) = yC - r2*sin(i);
        z_(i) = z0;

        % Calculamos el array de puntos de la elipse CON orientacion.
        tray(:,i) = rotmZYX*[x_(i); y_(i); z_(i)];

        **% Calculamos el array de orientacion
        alpha(i) = eul(1);
        beta(i) = eul(2);
        gamma(i) = eul(3);
        ori(:,i) = [alpha(i),beta(i),gamma(i)];**

        if(mod(i,100)==0)
            T = [[rotmZYX, [tray(:,i)]; [0 0 0 1]]];
            createFRAME(T,'m','To',2);
        end
    end

    plot3(tray(1,:),tray(2,:),tray(3,:),'g.'); grid; title('Trayectoria generada');
    xlabel('EJE X') 
    ylabel('EJE Y') 
    zlabel('EJE Z')
    trayectoria = [tray; ori]
end

[mariajesusmoragarcia]

o hay que mandarle en vez del array, la matriz de transformación?

[irivas-uma]

La salida de la función pueden ser 6 parámetros, pero la orientación no puede estar en ángulos de Euler, tienes que pasarlo a vector de rotación para poder mandárselo al robot.

[mariajesusmoragarcia]

Vale vale, creo que ya lo he entendido, he encontrado una función de Matlab que me hace la conversión directa de ángulos a vector de rotación, creo que sería así. El vector de rotación sería el mismo para todas las posiciones, ¿no?

Le adjunto el código modificado a ver si estoy en lo cierto, únicamente he añadido la función de conversión en el bucle.

function trayectoria = generador_trayectoria(p0, r1, r2, eul)
    close all;

    % Nos aseguramos que la pose p0 sean 1x3
    if size(p0,1) ~= 3
         error('p0 is not 3x1 vector');
    end

    % Nos aseguramos que el vector de orientación sea 1x3
    if size(eul,1) ~= 1
        error('eul is not 1x3 vector');
    end   

    % Matriz de orientación
    rotmZYX  = eul2rotm(eul);

    % Punto inicial
    x0 = p0(1);
    y0 = p0(2);
    z0 = p0(3);
    pini = rotmZYX*[x0; y0; z0];
    plot3(pini(1), pini(2), pini(3),'mO');
    hold on;

    % Sistema de referencia
    T = [[rotmZYX, [0;0;0]]; [0 0 0 1]];
    createFRAME(T,'m','To',2);

    t = 0:pi/200:2*pi;

    % Calculamos el centro de la elipse
    xC = x0 - r1*cos(0);
    yC = y0 - r2*sin(0);
    zC = 0;

    C = rotmZYX*[xC; yC; zC];
    plot3(C(1), C(2), C(3), 'b*')

    % Calculamos el array de puntos de la elipse sin orientacion
    for i = 1:length(t)
        x_(i) = xC - r1*cos(i);
        y_(i) = yC - r2*sin(i);
        z_(i) = z0;

        % Calculamos el array de puntos de la elipse CON orientacion.
        pos(:,i) = rotmZYX*[x_(i); y_(i); z_(i)];

        % Calculamos el array de orientacion
        angle_axis(:,i) = angle2rod(eul(1),eul(2),eul(3)); 

%         if(mod(i,100)==0)
%             T = [[rotmZYX, [pos(:,i)]; [0 0 0 1]]];
%             createFRAME(T,'m','To',2);
%         end
    end

    plot3(pos(1,:),pos(2,:),pos(3,:),'g.'); grid; title('Trayectoria generada');
    xlabel('EJE X') 
    ylabel('EJE Y') 
    zlabel('EJE Z')
    trayectoria = [pos; angle_axis]
end

Gracias de antemano, Un saludo.

[irivas-uma]

Mira a ver porque la función angle2rod pasa a vector de Euler-Rodrigues, que creo que no es la misma representación que vector de rotación. La función rotationMatrixToVector pasa de matriz de rotación a vector de rotación. Para comprobar si está bien o no coge una orientación que sepas identificar fácilmente, como paralela a algún eje, haz la transformación y pásasela al robot a ver si lo hace bien.

[mariajesusmoragarcia]

Vale, muchas gracias pues cambiaré la función y listo. En cuanto al simulador no voy a poder utilizarlo unos días, porque he tenido problemas con el ordenador y me lo están arreglando, se supone que me lo traían hoy o como muy tarde el miércoles. En cuanto me lo den, pruebo con los ejemplos.