Problema cinemática inverso. robótica

Problema cinemática inverso

Normalmente las trayectorias se expresan en coordenadas cartesianas que deben ser traducidas a variables de articulación. Esto es lo que se conoce como problemacinemático inverso. Las ecuaciones correspondientes para el robot RR son:

cosθ2= x2+ y2- L12- L222· L1·L2

tanθ1= y· L1+ L2· cosθ2- x· L2· sinθ2x· L1+ L2· cosθ2+ y· L2· sinθ2

Escribir un programa quesimule el movimiento del robot y dibuje la trayectoria para un movimiento rectilíneo entre dos puntos cualesquiera. Para ello se utilizará la función robot, del apartado anterior, y se construirá unanueva función pci que implemente las ecuaciones antes descritas.
function th = pci(px,py,L1,L2)
th2 = acos ((px^2+py^2-L1^2-L2^2)/2*L1*L2);
th1 = atan ((py*(L1+L2*cos(th2)) - px*L2*sin(th2)) /(px*(L1+L2*cos(th2)) + py*L2*sin(th2)));
th = [th1;th2];

Una vez creada la función pci, creamos un programa que simule el movimiento del robot y dibuje la trayectoria de éste.

Con este programa seprobarán las siguientes trayectorias:

1. (x1,y1) = (2,0) , (x2,y2) = (0,2)

2. (x1,y1) = (1,1) , (x2,y2) = (-1,0)

3. (x1,y1) = (1,0) , (x2,y2) = (-1,0)

Finalmente, se dibujarán lasfunciones temporales de las variables de articulación θ1=θ1(n) y θ2=θ2(n), donde n representa el instante de muestreo.

function trazado_rectilineo(x1,y1,x2,y2)
L1=1;
L2=1;
v=[];
w=[];
if x1-x2==0
forn=y1:0.1:y2
py=n;
px=x1+((x2-x1)/(y2-y1)*(py-y1));
p=[px;py];
th = pci(px,py,L1,L2);
v=[p(1) v];
w=[p(2) w];
x=[0 L1*cos(th(1)) v];
y=[0 L1*sin(th(1)) w];
plot(x,y)
axis([-2 2 -2 2]);pause
clf
end
else
for n=x1:0.1:x2
px=n;
py=y1+((y2-y1)/(x2-x1)*(px-x1));
p=[px;py];
th = pci(px,py,L1,L2);
v=[p(1) v];
w=[p(2) w];
x=[0 L1*cos(th(1)) v];
y=[0 L1*sin(th(1)) w];
plot(x,y)axis([-2 2 -2 2]);
pause
clf
end

function trazado_rectilineo4(x1,y1,x2,y2)
L1=1;
L2=1;
v=[];
w=[];
i=[2 1 0];
for n=1:3
px=i(n);
py=y1+((y2-y1)/(x2-x1)*(px-x1));
p=[px;py];
th =...