MATLAB Examples

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'); A=textscan(fid,'RB(0)(%*f);C=%f,%f,%f,%f,%f,%*f,%*f,%*f;','HeaderLines',0); 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^'); xlabel('Circle %');ylabel('J (m^4/rad^2)'); grid on; 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')