Code covered by the BSD License

# Inverse Optimal Functions for Motoman HP-3 Tip Precision

### Alan Jennings (view profile)

A population based optimization increases pointing precision for a planar robotic arm.

PointCluster2Angles_HP5_3

## Contents

% this takes a cluster and finds the joint angles for a given 3dPoint for
% the HP5

load('HP5_Circle_20120410_1.mat');


## Functions

% function [Theta_HP5]=PointCluster2Angles_HP5(Pnts,Cluster)
tia_list=linspace(0.95,1.195,15);
Cluster=Clusters{5};% used this one for example

% Cluster=Clusters{2}; % current circle is outside this domain
% Cluster=Clusters{4}; %Small cluster on side, Exceeds cluster range, first angle out of range also

% Cluster=Clusters{3}; % Acordians out like a stricking snake, goes through the circle
% Cluster=Clusters{1}; % approaches from underneath
% Cluster=Clusters{5}; %All cleanly in range

L_tool=4*0.0254; %length of tool from point P, MUST Match Cluster results

delta_angle_ass=(2*pi)/2^12; %assuming a 14 bit rotary encoder for 2pi radians

R_x=@(theta) [1,0,0,0;0,cos(theta),-sin(theta),0;0,sin(theta),cos(theta),0;0,0,0,1];
R_y=@(theta) [cos(theta),0,sin(theta),0;0,1,0,0;-sin(theta),0,cos(theta),0;0,0,0,1];
R_z=@(theta) [cos(theta),-sin(theta),0,0;sin(theta),cos(theta),0,0;0,0,1,0;0,0,0,1];
H_t=@(T) [eye(3), T;0,0,0,1];

% full model
P_1=@(theta)            R_z(theta(1))*H_t([0.1 ;0;0.3]);
P_2=@(theta) P_1(theta)*R_y(theta(2))*H_t([0   ;0;0.29]);
P_3=@(theta) P_2(theta)*R_y(theta(3))*H_t([0.3;0;0.085]/2);
P_4=@(theta) P_3(theta)*R_x(theta(4))*H_t([0.3;0;0.085]/2);
% this isn't does correctly, the axis for twist isn't where it's supposed to
% be, but I'm not useing that anyways
P_5=@(theta) P_4(theta)*R_y(theta(5))*H_t([0.09+L_tool;0;0]);
% simplified planar model
L_1s=0.29;L_2s=norm([.085,0.300]);L_3s=0.09+L_tool;
P_1s=@(theta) L_1s*[cos(theta(1));sin(theta(1))];
P_2s=@(theta) P_1s(theta(1))+L_2s*[cos(sum(theta(1:2)));sin(sum(theta(1:2)))];
P_3s=@(theta) P_2s(theta(1:2))+L_3s*[cos(sum(theta(1:3)));sin(sum(theta(1:3)))];

Four2three=@(Pnt) [eye(3), zeros(3,1)]*Pnt*[0;0;0;1];
Dist=@(Pnt) norm(Pnt-Four2three(P_1(Theta_s(Pnt))));

Theta_s=@(Pnt) atan2(Pnt(2),Pnt(1)); %Base swivel
Theta_2s=@(Pnt) -interp1(Cluster.y.',Cluster.x.',Dist(Pnt)^2,'pchip','extrap');
Theta_2=@(Pnt) -interp1(Cluster.y.',Cluster.x.',Dist(Pnt)^2,'pchip','extrap')+...
-[atan2(300,85),atan2(85,300)]; %final 2 angles
Theta_l=@(Pnt) atan2(norm(Pnt(1:2)-[1,0,0,0;0,1,0,0]*P_1(Theta_s(Pnt))*[0;0;0;1]),...
Pnt(3)-[0,0,1,0]*P_1(Theta_s(Pnt))*[0;0;0;1])-...
+atan2([0,1]*P_3s([0,Theta_2s(Pnt)]),[1,0]*P_3s([0,Theta_2s(Pnt)])); %Arm 1 angle


## Motoman solution

fid=fopen('HP5_pulse_trace_1.txt');
fclose(fid);
Down_rate=100;
Theta_txt=[A{1}(1:Down_rate:end)*0.000732421875,A{2}(1:Down_rate:end)*-0.00054931640625,...
A{3}(1:Down_rate:end)*0.000732421875,A{4}(1:Down_rate:end)*-0.0010986328125,...
A{5}(1:Down_rate:end)*0.0010986328125]*pi/180;
figure(7);plot(Theta_txt,'-+');legend({'1','2','3','4','5'})
title('Joint angles of standard path');


## Show the current solution

Theta_txt_xyz=zeros(size(Theta_txt,1),3);
Theta_txt_dis=zeros(size(Theta_txt,1),1);

figure(788);clf;daspect([1,1,1]);hold on;grid on;
for allie=1:size(Theta_txt,1)
% angles are given
% Theta_HP5=[Theta_s(Pnts),Theta_l(Pnts),Theta_2(Pnts)*[1,0,0;0,0,1]];

P_Plot_mat=[zeros(3,1),...
([eye(3), zeros(3,1)]*P_1(Theta_txt(allie,:).*-[1,1,1,1,1])*[0;0;0;1]),...
([eye(3), zeros(3,1)]*P_2(Theta_txt(allie,:).*-[1,1,1,1,1])*[0;0;0;1]),...
([eye(3), zeros(3,1)]*P_3(Theta_txt(allie,:).*-[1,1,1,1,1])*[0;0;0;1]),...
([eye(3), zeros(3,1)]*P_4(Theta_txt(allie,:).*-[1,1,1,1,1])*[0;0;0;1]),...
([eye(3), zeros(3,1)]*P_5(Theta_txt(allie,:).*-[1,1,1,1,1])*[0;0;0;1])];
plot3(P_Plot_mat(1,:),P_Plot_mat(2,:),P_Plot_mat(3,:),'co-');

xlabel('x');ylabel('y');zlabel('z');
grid on;view([39,30]);

title('Tracing circle with standard poses')

Theta_txt_xyz(allie,:)=P_Plot_mat(:,end).';
Theta_txt_dis2(allie)=norm(P_Plot_mat(:,end)-P_Plot_mat(:,2))^2;
end
plot3(Theta_txt_xyz(:,1),Theta_txt_xyz(:,2),Theta_txt_xyz(:,3),'kx');

figure(8);
plot(Theta_txt_dis2,'.');
title('Radial distance of the desired path')
Theta_txt_y=Y(bsxfun(@plus,-Theta_txt(:,[3,5]).',[atan2(300,85);atan2(85,300)])).';
Theta_txt_J=J(bsxfun(@plus,-Theta_txt(:,[3,5]).',[atan2(300,85);atan2(85,300)])).';

Theta_inv=zeros(size(Theta_txt,1),5);
Theta_inv_xyz=zeros(size(Theta_txt,1),3);
Theta_list=zeros(size(Theta_txt,1),2);
Dist_list=zeros(size(Theta_txt,1),1);
figure(789);clf;

for allie=1:size(Theta_txt_xyz,1)
Pnts=Theta_txt_xyz(allie,:).';
Pnt=Pnts;

% Add some preliminary checks on the point,
% such as being in the proper range for the cluster

% Find the point

Theta_HP5=[Theta_s(Pnts),Theta_l(Pnts),Theta_2(Pnts)*[1,0,0;0,0,1]];
% Theta_HP5*180/pi
Theta_list(allie,:)=Theta_2s(Pnts);
Dist_list(allie)=Dist(Pnts);

% Check angle limits, Possibly colision

% plot results
P_Plot_mat=[zeros(3,1),...
([eye(3), zeros(3,1)]*P_1(Theta_HP5.*[1,1,1,1,1])*[0;0;0;1]),...
([eye(3), zeros(3,1)]*P_2(Theta_HP5.*[1,1,1,1,1])*[0;0;0;1]),...
([eye(3), zeros(3,1)]*P_3(Theta_HP5.*[1,1,1,1,1])*[0;0;0;1]),...
([eye(3), zeros(3,1)]*P_4(Theta_HP5.*[1,1,1,1,1])*[0;0;0;1]),...
([eye(3), zeros(3,1)]*P_5(Theta_HP5.*[1,1,1,1,1])*[0;0;0;1])];
plot3(P_Plot_mat(1,:),P_Plot_mat(2,:),P_Plot_mat(3,:),'co-');
hold on;
plot3(Pnts(1,:),Pnts(2,:),Pnts(3,:),'k*');
daspect([1,1,1])
xlabel('x');ylabel('y');zlabel('z');
% axis tight;
grid on;view([39,30]);
title('Tracing circle with optimized poses')

Theta_inv(allie,:)=Theta_HP5;
Theta_inv_xyz(allie,:)=P_Plot_mat(:,end).';
% drawnow;
end

Theta_inv_y=Y(bsxfun(@plus,Theta_inv(:,[3,5]).',[atan2(300,85);atan2(85,300)])).';
Theta_inv_J=J(bsxfun(@plus,Theta_inv(:,[3,5]).',[atan2(300,85);atan2(85,300)])).';


## Compare results

figure(790);clf;%hold on;
subplot(2,1,1);
plot(linspace(0,1,length(Theta_inv_y)),Theta_inv_y,'r^',...
linspace(0,1,length(Theta_txt_y)),Theta_txt_y,'g+');
xlabel('Circle %');ylabel('y_d');grid on;
ylim([0,.25]);legend({'Optimal Inverse','Standard'})
subplot(2,1,2);
plot(linspace(0,1,length(Theta_inv_y)),Theta_inv_J,'r^',...
linspace(0,1,length(Theta_txt_y)),Theta_txt_J,'g+');
% plot(linspace(0,1,length(Theta_inv_y)),Theta_txt_J./Theta_inv_J,'r^');
xlabel('Circle %');ylabel('J'); grid on;


## Compare results

figure(791);clf;%hold on;
subplot(2,2,1);
plot(linspace(0,1,length(Theta_inv_y)),Theta_inv_y,'r^',...
linspace(0,1,length(Theta_txt_y)),Theta_txt_y,'g+');
% xlabel('Circle %');
ylabel('y_d (m^2)');grid on;
ylim([0,.25]); %legend({'Optimal Inverse','Standard'})
title('Output')

subplot(2,2,2);
plot(linspace(0,1,length(Theta_inv_y)),1000*sqrt(Theta_inv_y),'r^',...
linspace(0,1,length(Theta_txt_y)),1000*sqrt(Theta_txt_y),'g+');
% xlabel('Circle %');
ylabel('y_d^1^/^2 (mm)');grid on;
% ylim([0,.5]);legend({'Optimal Inverse','Standard'},'location','se')
ylim([0,500]);legend({'Optimal Inverse','Standard'},'location','se')
title('Distance')

subplot(2,2,3);
plot(linspace(0,1,length(Theta_inv_y)),Theta_inv_J,'r^',...
linspace(0,1,length(Theta_txt_y)),Theta_txt_J,'g+');
% plot(linspace(0,1,length(Theta_inv_y)),Theta_txt_J./Theta_inv_J,'r^');
ylim([0,0.2]);
title('Cost')

subplot(2,2,4);
plot(linspace(0,1,length(Theta_inv_y)),1000*sqrt(Theta_inv_J./(10*Theta_inv_y))*delta_angle_ass,'r^',...
linspace(0,1,length(Theta_txt_y)),1000*sqrt(Theta_txt_J./(10*Theta_txt_J))*delta_angle_ass,'g+');
% plot(linspace(0,1,length(Theta_inv_y)),Theta_txt_J./Theta_inv_J,'r^');
% xlabel('Circle %');ylabel('(J/10y_d)^1^/^2 d\theta, m/pulse'); grid on;
xlabel('Circle %');ylabel('(J/10y_d)^1^/^2 d\theta (mm)'); grid on;
% ylim([0,0.01]);
title('    Distance error bound')


## write results to file

% fid=fopen('HP_Circle_2012_0410_1b.txt','wt');
% temp_a=[round(linspace(0,5.89,size(Theta_inv,1))*100).'/100,...
%     round(bsxfun(@times,-Theta_inv*180/pi,[0.000732422	-0.000549316	0.000732422	-0.001098633	0.001098633].^-1))].';
% fprintf(fid,'RB(0)(%06.3g);C=%i,%i,%i,%i,%i,0,0,0;\n',temp_a);
% fclose(fid);

% temp_a=round(bsxfun(@times,-Theta_inv*180/pi,[0.000732422	-0.000549316	0.000732422	-0.001098633	0.001098633].^-1));
% csvwrite('HP_Circle_2012_0410_1d.csv',temp_a(:,[1:end,4]))

figure(795);clf
plot(Cluster.x(1,:),Cluster.x(2,:),'ro');hold on;
temp_plot=(-Theta_list(:,1)>=(min(X1(:))-0.1))&(-Theta_list(:,1)<=(max(X1(:))+0.1))&...
(-Theta_list(:,2)>=(min(X2(:))-0.1))&(-Theta_list(:,2)<=(max(X2(:))+0.1));
plot(-Theta_list(temp_plot,1),-Theta_list(temp_plot,2),'b-+');
plot(-Theta_list(~temp_plot,1),-Theta_list(~temp_plot,2),'g-x');

axis([min(X1(:)),max(X1(:)),min(X2(:)),max(X2(:))]);
daspect([1,1,1])
xlabel('\theta_3');ylabel('\theta_5')
title('Path and cluster shown in joint space')

figure(797);clf;
plot(find(abs(Theta_inv(:,1))<=(170*pi/180))+0/5,180/pi*Theta_inv(abs(Theta_inv(:,1))<=(170*pi/180),1),'r-+')
hold on;
plot(find((Theta_inv(:,2)<=(150*pi/180))&(Theta_inv(:,2)>=(-45*pi/180)))+1/5,180/pi*Theta_inv((Theta_inv(:,2)<=(150*pi/180))&(Theta_inv(:,2)>=(-45*pi/180)),2),'g-+')
plot(find((-Theta_inv(:,3)<=(210*pi/180))&(-Theta_inv(:,3)>=(-152*pi/180)))+2/5,-180/pi*Theta_inv((-Theta_inv(:,3)<=(210*pi/180))&(-Theta_inv(:,3)>=(-152*pi/180)),3),'b-+')
plot(find(abs(Theta_inv(:,4))<=(190*pi/180))+3/5,180/pi*Theta_inv(abs(Theta_inv(:,4))<=(190*pi/180),4),'m-+')
plot(find(abs(Theta_inv(:,5))<=(125*pi/180))+4/5,180/pi*Theta_inv(abs(Theta_inv(:,5))<=(125*pi/180),5),'c-+');

plot(find(~(abs(Theta_inv(:,1))<=(170*pi/180)))+0/5,180/pi*Theta_inv(~(abs(Theta_inv(:,1))<=(170*pi/180)),1),'r-o')
plot(find(~((Theta_inv(:,2)<=(150*pi/180))&(Theta_inv(:,2)>=(-45*pi/180))))+1/5,180/pi*Theta_inv(~((Theta_inv(:,2)<=(150*pi/180))&(Theta_inv(:,2)>=(-45*pi/180))),2),'g-o')
plot(find(~((-Theta_inv(:,3)<=(210*pi/180))&(-Theta_inv(:,3)>=(-152*pi/180))))+2/5,-180/pi*Theta_inv(~((-Theta_inv(:,3)<=(210*pi/180))&(-Theta_inv(:,3)>=(-152*pi/180))),3),'b-o')
plot(find(~(abs(Theta_inv(:,4))<=(190*pi/180)))+3/4,180/pi*Theta_inv(~(abs(Theta_inv(:,4))<=(190*pi/180)),4),'m-o')
plot(find(~(abs(Theta_inv(:,5))<=(125*pi/180)))+4/5,180/pi*Theta_inv(~(abs(Theta_inv(:,5))<=(125*pi/180)),5),'c-o');

plot([1,size(Theta_inv,1),size(Theta_inv,1),1],170*[1,1,-1,-1],'r-.');
hold on;
plot([1,size(Theta_inv,1),size(Theta_inv,1),1],[150*[1,1],-45*[1,1]],'g-.');
plot([1,size(Theta_inv,1),size(Theta_inv,1),1],[210*[1,1],-152*[1,1]],'b-.');
plot([1,size(Theta_inv,1),size(Theta_inv,1),1],190*[1,1,-1,-1],'m-.');
plot([1,size(Theta_inv,1),size(Theta_inv,1),1],125*[1,1,-1,-1],'c-.');

% legend({'1','2','3','4','5'})
text(1,-0,{'1 r','2 g','3 b','4 m','5 c'})
title('Verify that joints stay within limits')