%% EXAMEN DICIEMBRE 2019 - PROBLEMA 2: RESOLUCIÓN COMPLETA PASO A PASO % Asignatura: Robótica (GITI - Escuela Superior de Ingenieros de Sevilla) % % Robot de 4 GDL: 3 rotaciones (theta1, theta2, theta3) + 1 prismática (d4) % Extremo: Cámara de visión % % Este script resuelve de forma analítica y numérica: % a) Cinemática Directa mediante convención Denavit-Hartenberg (D-H) % b) Cinemática Inversa completa (4 soluciones) con d4 = 0 % c) Jacobiano y Análisis de Singularidades clear; clc; close all; fprintf('====================================================================\n'); fprintf(' EXAMEN DICIEMBRE 2019 - ROBÓTICA - PROBLEMA 2 (CINEMÁTICA Y DH) \n'); fprintf('====================================================================\n\n'); %% 1. DEFINICIÓN DE VARIABLES SIMBÓLICAS syms theta1 theta2 theta3 d4 real syms L1 L2 L3 real %% 2. FUNCIÓN DE MATRIZ ELEMENTAL DENAVIT-HARTENBERG ESTÁNDAR % A_i = Rot(z, theta) * Tras(z, d) * Tras(x, a) * Rot(x, alpha) dh = @(theta, d, a, alpha) [ cos(theta), -cos(alpha)*sin(theta), sin(alpha)*sin(theta), a*cos(theta); sin(theta), cos(alpha)*cos(theta), -sin(alpha)*cos(theta), a*sin(theta); 0, sin(alpha), cos(alpha), d; 0, 0, 0, 1 ]; %% 3. TABLA DE PARÁMETROS D-H PASO A PASO % Eje Z0: E1 (vertical hacia arriba en la base) % Eje Z1: E2 (vertical hacia arriba en la articulación 2) % Eje Z2: E3 (horizontal, en dirección transversal perpendicular a L2) % Eje Z3: Eje de la articulación prismática a lo largo del brazo de la cámara % % Articulación 1: Rotación theta1, d1 = 0, a1 = L1, alpha1 = 0 % Articulación 2: Rotación theta2, d2 = 0, a2 = L2, alpha2 = +pi/2 % Articulación 3: Rotación (theta3 + pi/2), d3 = 0, a3 = 0, alpha3 = +pi/2 % Articulación 4: Prismática (L3 + d4), theta4 = 0, a4 = 0, alpha4 = 0 fprintf('--- TABLA DE PARÁMETROS D-H ---\n'); fprintf('i | theta_i | d_i | a_i | alpha_i\n'); fprintf('-----------------------------------------------\n'); fprintf('1 | theta1 | 0 | L1 | 0\n'); fprintf('2 | theta2 | 0 | L2 | +90 deg\n'); fprintf('3 | theta3 + 90 deg | 0 | 0 | +90 deg\n'); fprintf('4 | 0 | L3 + d4 | 0 | 0\n\n'); % Matrices homogéneas elementales A1 = dh(theta1, 0, L1, 0); A2 = dh(theta2, 0, L2, sym(pi)/2); A3 = dh(theta3 + sym(pi)/2, 0, 0, sym(pi)/2); A4 = dh(0, L3 + d4, 0, 0); % Transformación total de la base al extremo de la cámara T01 = A1; T02 = simplify(T01 * A2); T03 = simplify(T02 * A3); T04 = simplify(T03 * A4); %% APARTADO a): POSICIÓN DEL EXTREMO (CINEMÁTICA DIRECTA) p_extremo = T04(1:3, 4); p_extremo = simplify(p_extremo); fprintf('--- APARTADO a): POSICIÓN DEL EXTREMO (CINEMÁTICA DIRECTA) ---\n'); fprintf('x(q) = %s\n', char(p_extremo(1))); fprintf('y(q) = %s\n', char(p_extremo(2))); fprintf('z(q) = %s\n\n', char(p_extremo(3))); % En forma simplificada equivalente: % x = L1*cos(theta1) + (L2 + (L3+d4)*cos(theta3))*cos(theta1+theta2) % y = L1*sin(theta1) + (L2 + (L3+d4)*cos(theta3))*sin(theta1+theta2) % z = (L3+d4)*sin(theta3) %% APARTADO c): JACOBIANO Y SINGULARIDADES % Caso c.1: Manipulador con d4 = 0 (3 GDL como en apartado b) q3_vars = [theta1; theta2; theta3]; p_3gdl = subs(p_extremo, d4, 0); J3 = jacobian(p_3gdl, q3_vars); det_J3 = simplify(det(J3)); det_J3_fact = factor(det_J3); fprintf('--- APARTADO c): SINGULARIDADES (3 GDL, d4=0) ---\n'); fprintf('Determinante del Jacobiano:\n det(J) = %s\n', char(det_J3)); fprintf('Determinante factorizado:\n det(J) = %s\n\n', char(det_J3_fact)); fprintf('Condiciones de singularidad (det(J) = 0):\n'); fprintf(' 1. sin(theta2) = 0 ==> theta2 = 0 o 180 deg (Alineamiento de eslabones 1 y 2 en el plano)\n'); fprintf(' 2. cos(theta3) = 0 ==> theta3 = +/- 90 deg (Brazo de camara completamente vertical)\n'); fprintf(' 3. L2 + L3*cos(theta3) = 0 ==> cos(theta3) = -L2/L3 (Extremo sobre el eje vertical E2)\n\n'); %% APARTADO b): CINEMÁTICA INVERSA NUMÉRICA Y ANALÍTICA (d4 = 0) fprintf('--- APARTADO b): CINEMÁTICA INVERSA (d4 = 0) ---\n'); % Valores de prueba geométricos: L1_val = 0.5; L2_val = 0.4; L3_val = 0.3; % Punto objetivo en espacio de trabajo x_des = 0.6; y_des = 0.4; z_des = 0.15; fprintf('Resolviendo cinemática inversa para el punto objetivo: [x=%.2f, y=%.2f, z=%.2f]\n', x_des, y_des, z_des); fprintf('Dimensiones del robot: L1=%.2f, L2=%.2f, L3=%.2f\n\n', L1_val, L2_val, L3_val); % 1. Despeje de theta3 a partir de z = L3 * sin(theta3) s3 = z_des / L3_val; if abs(s3) > 1 error('El punto está fuera del rango alcanzable en Z (|z| > L3)'); end % Dos posibles ramas para theta3 (hacia adelante y hacia atrás) c3_pos = sqrt(1 - s3^2); c3_neg = -sqrt(1 - s3^2); theta3_sols = [atan2(s3, c3_pos), atan2(s3, c3_neg)]; c3_sols = [c3_pos, c3_neg]; sol_count = 0; soluciones = []; for k3 = 1:2 th3 = theta3_sols(k3); c3 = c3_sols(k3); % Radio efectivo del segundo tramo en el plano horizontal R2 = L2_val + L3_val * c3; % Problema planar 2R para theta2: x^2 + y^2 = L1^2 + R2^2 + 2*L1*R2*cos(theta2) c2 = (x_des^2 + y_des^2 - L1_val^2 - R2^2) / (2 * L1_val * R2); if abs(c2) <= 1 % Dos ramas para theta2 (codo arriba / codo abajo) s2_pos = sqrt(1 - c2^2); s2_neg = -sqrt(1 - c2^2); theta2_cands = [atan2(s2_pos, c2), atan2(s2_neg, c2)]; for k2 = 1:2 th2 = theta2_cands(k2); % Despeje de theta1 % x = (L1 + R2*cos(th2))*cos(th1) - (R2*sin(th2))*sin(th1) % y = (L1 + R2*cos(th2))*sin(th1) + (R2*sin(th2))*cos(th1) k_a = L1_val + R2 * cos(th2); k_b = R2 * sin(th2); th1 = atan2(y_des, x_des) - atan2(k_b, k_a); sol_count = sol_count + 1; soluciones(:, sol_count) = [th1; th2; th3]; % Comprobación cinemática directa p_check = [ L1_val*cos(th1) + (L2_val + L3_val*cos(th3))*cos(th1 + th2); L1_val*sin(th1) + (L2_val + L3_val*cos(th3))*sin(th1 + th2); L3_val*sin(th3) ]; err = norm(p_check - [x_des; y_des; z_des]); fprintf('Solución %d:\n', sol_count); fprintf(' theta1 = %8.3f deg\n', rad2deg(th1)); fprintf(' theta2 = %8.3f deg\n', rad2deg(th2)); fprintf(' theta3 = %8.3f deg\n', rad2deg(th3)); fprintf(' Error de verificación: %.2e m\n\n', err); end else fprintf('Para rama theta3 = %.2f deg, c2 = %.3f fuera de rango [-1, 1].\n', rad2deg(th3), c2); end end fprintf('Total de soluciones cinemáticas encontradas: %d\n', sol_count);