Code sometimes getting stuck in an infinite loop when changing target point for 2d simulation of robotic arm. HELP
2 views (last 30 days)
Show older comments
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc; clear;
pt = [1;2]; % target point (in column matrix) l1 = 1; l2 = 1; l3 = 1; l4 = 1; theta1 = pi/6; theta2 = pi/6; theta3 = pi/6; theta4 = pi/6; dt =.01; %time step counter = 0; figure(1) title('Planar Robot') axis manual % set axis to exact manual value( hold on % does not erase previous graphs %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% initialization of data num_of_link = 4; %number of link xdata = (0:l1+l2+l3+l4);
for i = 1:num_of_link+1 ydata(i) = 0; end for i = 1:num_of_link angledata(i) = 0; end
error = dist([xdata(num_of_link+1) ydata(num_of_link+1)], pt); while (error > 0.5) iteration = num_of_link + 1; while (iteration > 1) %% CCD Algorthm
pe = [xdata(num_of_link+1); ydata(num_of_link+1)];
pc = [xdata(iteration-1); ydata(iteration-1)];
a = (pe - pc)/norm(pe-pc);
b = (pt - pc)/norm(pt-pc);
theta = acosd(dot(a, b));
direction = cross([a(1) a(2) 0],[b(1) b(2) 0]);
if direction(3) < 0
theta = -theta; %defines whether or not it's a minus or positive value
end
if (theta > 30)
theta = 30;
elseif (theta < -30)
theta = -30;
end
angledata(iteration-1) = theta;
iteration = iteration - 1;
%%let's do the rotation!
for i = 1:num_of_link-1
R = [cosd(angledata(i)) -sind(angledata(i)); sind(angledata(i)) cosd(angledata(i))]; % rotation matrix
% p' = R * (p - c) + c
temp = R * ([xdata(i+1); ydata(i+1)] - [xdata(i); ydata(i)]) + [xdata(i); ydata(i)];
xdata(i+1) = temp(1);
ydata(i+1) = temp(2);
angledata(i+1) = angledata(i+1) + angledata(i);
xdata(i+2) = xdata(i+1)+1;
ydata(i+2) = ydata(i+1);
end
% end effector rotation
i = i+1;
R = [cosd(angledata(i)) -sind(angledata(i)); sind(angledata(i)) cosd(angledata(i))]; % rotation matrix
% p' = R * (p - c) + c
temp = R * ([xdata(i+1); ydata(i+1)] - [xdata(i); ydata(i)]) + [xdata(i); ydata(i)];
xdata(i+1) = temp(1);
ydata(i+1) = temp(2);
end
error = dist([xdata(num_of_link+1) ydata(num_of_link+1)], pt);
disp(error);
end
%Calculate the location of the middle three joints pointl1 = [l1*cos(theta1) ; l1*sin(theta1)]; pointl2 = pointl1 + [l2*cos(theta1+theta2);l2*sin(theta1+theta2)]; pointl3 = pointl2 + [l3*cos(theta1+theta2+theta3);l3*sin(theta1+theta2+theta3)];
%Plot if (mod(counter,10)==0) %plots every 10 iterations axis([0 10 0 10]) axis square line([0,pointl1(1)],[0,pointl1(2,1)]) hold on line([pointl1(1),pointl2(1)],[pointl1(2,1),pointl2(2,1)]) hold on line([pointl2(1),pointl3(1)],[pointl2(2,1),pointl3(2,1)]) line([pointl3(1),temp(1)],[pointl3(2,1),temp(2,1)])
plot(temp(1),temp(2),'o') pause(.1)
end
disp('Simulate');
0 Comments
Answers (0)
See Also
Categories
Find more on Coordinate Transformations in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!