I have a problem with my code because this error "Subscripted assignment dimension mismatch." It is a program for a robot of two degrees of freedom. Could you helpme?
clc; clear all; close all; clear imports
import ETS3.*
l1=0.2;
l2=0.2;
q1=[-pi/2 pi/2]
q2=[-pi/2 pi/2]
%%%%%%%%%%%%%%%%%%%Theta--
link_1 = Revolute('qlim',q1,'d',0.1,'a',0.2,'alpha',0,'offset',pi/2)
link_2 = Revolute('qlim',q2,'d',0,'a',0.2,'alpha',0,'offset',0)
%links=[link_1]
links=[link_1,link_2]
Yo_Robot=SerialLink(links,'name','Robot que Escribe','plotopt',{'view',[140 40],'workspace',[-0.5 0.5 -0.5 0.5 0 0.2]})
%Yo_Robot.teach
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cinemática Inversa
syms q1 q2 theta d a alpha %definir como simbólico
%para el eslabon 1
theta(1)=q1+pi/2; d(1)=0.1; a(1)=0.2; alpha(1)=0;
%para el eslabon 2
theta(2)=q2; d(2)=0; a(2)=0.2; alpha(2)=0;
disp('Parámetros Denavit-Hartenberg, simbólicos:')
D_H=[theta.',d.',a.',alpha.']
for i=1:1:2 %calcular matriz por cada transformación {Si-1} -> {Si}
%crear Hipermatriz
A(:,:,i)=[
cos(theta(i)) , -cos(alpha(i))*sin(theta(i)) , sin(alpha(i))*sin(theta(i)) , a(i)*cos(theta(i));
sin(theta(i)) , cos(alpha(i))*cos(theta(i)) , -sin(alpha(i))*cos(theta(i)) , a(i)*sin(theta(i));
0 , sin(alpha(i)) , cos(alpha(i)) , d(i)
0 , 0 , 0 , 1
];
for f=1:1:2 %para eliminar error númerico
for c=1:1:2
if abs(double(coeffs(A(f,c,i))))<1e-16 % aproximadamente = 0
A(f,c,i)=0;
end
end
end
end
for i=1:1:2 %mostrar todas las matrices de transformación
disp(['Matriz de transformación: ',num2str(i-1),'A',num2str(i)])
A(:,:,i)
end
disp('Matriz de transformación: 0T2')
T_0a2 = simplify(A(:,:,1)*A(:,:,2)) %matriz de transformación desde 0 a 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% JACOBIANA DIRECTA
% la posición del efector final es
p=T_0a2(1:2,4)
% la matriz Jacobiana es J = dp/dq, donde p es vector y q es vector
q=[q1; q2];
J=jacobian(p,q)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% JACOBIANA INVERSA
% Calcular la cinemática inversa del robot para tener q en función de x, y, z
syms px py
syms q1 q2
q1 = atan2(-px,py)-atan2((0.2*sin(q2)),(0.2+0.2*cos(q2)))
q2 = acos((px^2+py^2-0.2^2-0.2^2)/(2*0.2*0.2))
% [s_q1,s_q2]=solve(px==T_0a2(1,4),py==T_0a2(2,4),q1,q2)
s_q1=matlabFunction(q1)
s_q2=matlabFunction(q2)
J_fun=matlabFunction(J);
%partiendo de una velocidad del extremo del robot
t=0:0.05:1;
ax=0.1; %aceleracion costante para mover 0.3 m en 5s partiendo de vx=0
ay=-0.05; %aceleracion costante para mover 0.3 m en 5s partiendo de vy=0
vx1=ax*t;
vy1=ay*t;
pxi=0;
pyi=0.3;
px1=pxi+0.5*ax*t.^2;
py1=pyi+0.5*ay*t.^2;
%velocidad constante
t=0:0.05:1;
vx2=vx1(length(t)); %m/s
vy2=vy1(length(t)); %m/s
px2=px1(length(t))+vx2*t; % x no puede ser cero cuando q1=q2=0
py2=py1(length(t))+vy2*t;
t=0:0.05:1;
ax=-0.1; %aceleracion costante para mover 0.3 m en 5s partiendo de vx=0
ay=0.05; %aceleracion costante para mover 0.3 m en 5s partiendo de vy=0
vx3=vx2+ax*t;
vy3=vy2+ay*t;
px3=px2(length(t))+vx2*t+0.5*ax*t.^2; % x no puede ser cero cuando q1=q2=0
py3=py2(length(t))+vy2*t+0.5*ay*t.^2;
px=[px1 px2 px3];
py=[py1 py2 py3];
clear q1 q2
for i=1:1:length(t)
resp=s_q2(px(i),py(i));
q2(i)=resp(1);
resp=s_q1(px(i),py(i),q2(i));
q1(i)=resp(1);
end
q=real([q1' q2'])
Yo_Robot.plot(q,'view',[0 90])
for i=1:1:length(t) %crear Hipermatriz con todas las Jacobianas directas e inversas
J_eva(:,:,i)=J_fun(q1(i),q2(i));
J_eva_inv(:,:,i)=inv(J_eva(:,:,i));
end
A1=vx2;
A2=vy2;
for i=1:1:length(t)
vx2(i)=A1;
vy2(i)=A2;
end
vx=[vx1 vx2 vx3];
vy=[vy1 vy2 vy3];
v=[vx;vy];
clear q_p
for i=1:1:length(t)
q_p(:,i)=J_eva_inv(:,:,i)*v; %notense las singularidades. No inversa por det=0 en det(J_eva(:,:,1))
% singularidad se puede resolver con la función pinv
end
t=0:0.05:9.1;
figure
subplot(3,1,1)
plot(t,q_p(1,:)','r',t,q_p(2,:)'*150)
subplot(3,1,2)
plot(t,q1,t,q2)
subplot(3,1,3)
plot(t,vx,t,vy)
