robodemo.m

一个matlab编写的robot demo程序

%%%%%Robot Animation of linear trajectory with real time error plot
%%%%%This program will animate the 3D model of a RPP (revolute, prismatic, prismatic)
%joint robot while it follows a simple 3D linear trajectory in space. The RPP robot
%is controlled in jount space with smooth continuous 3rd and 4th order splines that
%provide continuous velocity, and acceleration without violating joint constraints
%for maximum torque and speed. The trajectory was arbitrairly chosen to allow 5
%inverse kinematic precision points along the path. Notice as the robot animates
%along the linear 3D line, MATLAB will plot the error of the end-effector which is
%the difference between the commanded position and the perpenducular distance to the
%actual 3D line. Watch for the five points of accuracy (where the error is zero). 
%%%%%Note:	This program would look cleaner if you wrote functions for the translate,
%		rotate, tmat, and linkdata calculations, but it is easier for people to
%		download when it is all in one file.
%%%%%References	1)Writen by:	Zach Piner		[e-mail]
%						Adam Hansel		[e-mail]
%						March 11, 1998
%						2)Introduction to Robotics:Mechanics and Control, John J. Craig,
%						2nd ed.
%						3)"Design Modeling, Trajectory Calculation and General Calibration
%						of a Three Degree of Freedom Cylindrical Robot",Masters Thesis,
%						Washington State University 1986, Donald Riley (Professor of
%						Mechanical Engineering, Walla Walla College, Washington)
%MATLAB V5.1
%%Feel free to email us with any question about our program.


clear all
%Equations for trajectory (Riley)
res=50;
t1range=1.887;
t2range=.915;
t3range=.518;
t4range=.374;

ts1=0:20/50:20;
ts2=21:1:40;
ts3=41:1/50:60;

t1=0:t1range/res:t1range;
t2=0:t2range/res:t2range;
t3=0:t3range/res:t3range;
t4=0:t4range/res:t4range;

%Spline data (Joint equations)
%Position
t11=18.27*t1.^3-5.836*t1.^4;
t12=1.454*t1.^3-.346*t1.^4;
t13=.783*t1.^3-.143*t1.^4;

t21=48.76+38.34*t2-21.18*t2.^2+7.155*t2.^3;
t22=5.375+6.229*t2+.843*t2.^2-1.34*t2.^3;
t23=3.447+4.535*t2+1.389*t2.^2-1.411*t2.^3;

t31=71.605+17.62*t3-1.494*t3.^2+20.534*t3.^3;
t32=10.75+4.404*t3-2.824*t3.^2+27.615*t3.^3;
t33=7.669+3.518*t3-2.472*t3.^2+23.19*t3.^3;

t41=83.195+32.61*t4+30.39*t4.^2-341.65*t4.^3+420.57*t4.^4;
t42=16.125+23.75*t4+40.12*t4.^2-312.8*t4.^3+370.6*t4.^4;
t43=12.055+19.61*t4+33.45*t4.^2-259.51*t4.^3+307.12*t4.^4;


tha1=[t11,t21,t31,t41];
ddd2=[t12,t22,t32,t42];
ddd3=[t13,t23,t33,t43];
%End position
%Velocity
v11=3*18.27*t1.^2-4*5.836*t1.^3;
v12=3*1.454*t1.^2-4*.3460*t1.^3;
v13=3*.7830*t1.^2-4*.1430*t1.^3;

v21=38.34-2*21.18*t2+3*7.155*t2.^2;
v22=6.229+2*.8430*t2-3*1.340*t2.^2;
v23=4.535+2*1.389*t2-3*1.411*t2.^2;

v31=17.62-2*1.494*t3+3*20.534*t3.^2;
v32=4.404-2*2.824*t3+3*27.615*t3.^2;
v33=3.518-2*2.472*t3+3*23.190*t3.^2;

v41=32.61+2*30.39*t4-3*341.65*t4.^2+4*420.57*t4.^3;
v42=23.75+2*40.12*t4-3*312.80*t4.^2+4*370.6*t4.^3;
v43=19.61+2*33.45*t4-3*259.51*t4.^2+4*307.12*t4.^3;


vtha1=[v11,v21,v31,v41];
vd2=[v12,v22,v32,v42];
vd3=[v13,v23,v33,v43];
%End velocity
%Acceleration
a11=3*2*18.27*t1-4*3*5.836*t1.^2;
a12=3*2*1.454*t1-4*3*.3460*t1.^2;
a13=3*2*.7830*t1-4*3*.1430*t1.^2;

a21=-2*21.18+3*2*7.155*t2;
a22=2*.8430-3*2*1.340*t2;
a23=2*1.389-3*2*1.411*t2;

a31=-2*1.494+3*2*20.534*t3;
a32=-2*2.824+3*2*27.615*t3;
a33=-2*2.472+3*2*23.190*t3;

a41=2*30.39-3*2*341.65*t4+4*3*420.57*t4.^2;
a42=2*40.12-3*2*312.80*t4+4*3*370.6*t4.^2;
a43=2*33.45-3*2*259.51*t4+4*3*307.12*t4.^2;


atha1=[a11,a21,a31,a41];
ad2=[a12,a22,a32,a42];
ad3=[a13,a23,a33,a43];
%End acceleration
%Time Vector
time=[t1,t1range+t2,t1range+t2range+t3,t1range+t2range+t3range+t4];
%End of trajectory calculations

%animate 3D RRP robot

%this will make a link l,w,h (x,y,z)
%link one dimensions
l=14;
w=3;
h=1;
xdata=[0 l l 0 0 l l 0];
ydata=[0 0 w w 0 0 w w];
zdata=[0 0 0 0 h h h h];
one=[1 1 1 1 1 1 1 1];
linkdata=[xdata;ydata;zdata;one];
%end link dimensions

%Translate link (Craig)
x=-7;
y=-1.5;
z=-3;
t=[1 0 0 x; 0 1 0 y; 0 0 1 z; 0 0 0 1];
%end translate

%Rotate link (Craig)
THETA=90;
THETA = THETA*pi/180;  %Note: THETA is in radians.
c = cos(THETA);
s = sin(THETA);
Rz = [c -s 0 0; s c 0 0; 0 0 1 0; 0 0 0 1];
%end rotate

p1=t*linkdata;  %center link
p2=Rz*p1; %for hub simulation

l=1;
w=5;
h=26;
xdata=[0 l l 0 0 l l 0];
ydata=[0 0 w w 0 0 w w];
zdata=[0 0 0 0 h h h h];
one=[1 1 1 1 1 1 1 1];
linkdata=[xdata;ydata;zdata;one];

%Translate link
x=-.5;
y=-2.5;
z=-3;
t=[1 0 0 x; 0 1 0 y; 0 0 1 z; 0 0 0 1];
%end translate

p3=t*linkdata; %for large upright bar
l1i=[p1 p2 p3];
l1=[p1 p2 p3];
lfs=[1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8];
link1faces=[lfs;lfs+8;lfs+16];

%link two
l=6.6;
w=7.5;
h=.375;
xdata=[0 l l 0 0 l l 0];
ydata=[0 0 w w 0 0 w w];
zdata=[0 0 0 0 h h h h];
one=[1 1 1 1 1 1 1 1];
linkdata=[xdata;ydata;zdata;one];

%Translate link
x=-1.375;
y=-7.5/2;
z=1;
t=[1 0 0 x; 0 1 0 y; 0 0 1 z; 0 0 0 1];
%end translate

l2=linkdata;
l2=t*l2;
l2i=l2;

%link three

l=1;
w=1;
h=24;
xdata=[0 l l 0 0 l l 0];
ydata=[0 0 w w 0 0 w w];
zdata=[0 0 0 0 h h h h];
one=[1 1 1 1 1 1 1 1];
linkdata=[xdata;ydata;zdata;one];

%Translate link
x=-.5;
y=-.5;
z=-24+3.75;
t=[1 0 0 x; 0 1 0 y; 0 0 1 z; 0 0 0 1];
%end translate

%(tmat)Equation 3.6 (Craig)
alpha=90;		%Note: alpha is in radians.
a=3.85;
d=0;
thetaa=0;		%Note: theta is in radians.
alpha = alpha*pi/180;    
thetaa = thetaa*pi/180;    
c = cos(thetaa);
s = sin(thetaa);
ca = cos(alpha);
sa = sin(alpha);
T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
%end tmat

l3=linkdata;
l3=t*l3;
l3i=T*l3;

figure(4)
clf
   subplot(2,1,1)
theta1=tha1';
d2=ddd2';
d3=ddd3';
%Setting up handles for animation

hdl3=patch('faces',lfs,'vertices',[l3i(1,:)' l3i(2,:)' l3i(3,:)'],'FaceColor','g');
hdl2=patch('faces',lfs,'vertices',[l2i(1,:)' l2i(2,:)' l2i(3,:)'],'FaceColor','r');
hdl1=patch('faces',link1faces,'vertices',[l1i(1,:)' l1i(2,:)' l1i(3,:)'],'FaceColor','b');
hold on
set(gcf,'renderer','zbuffer');
grid on
view(52,30)
title('Linear Trajectory of RPP Robot');
xlabel('X')
ylabel('Y')
zlabel('Z')
axis('equal')
axis('square')
axis([-15 15 -15 15 -5 25]);
rotate3d on
%setting up handle for path of end link
plt=plot3(0,0,0,'r');
x=[3.85,20.25];
y=[-3.75,3.85];
z=[0,21.5];
%this is a loop to calculate the error at one degree increments
for i=1:1:size(theta1);
   theta=theta1(i,:);
   dd2=d2(i,:);
   dd3=d3(i,:);
   %(tmat)Equation 3.6 (Craig)
alpha=0;		%Note: alpha is in radians.
a=0;
d=0;
thetaa=theta;		%Note: theta is in radians.
alpha = alpha*pi/180;    
thetaa = thetaa*pi/180;    
c = cos(thetaa);
s = sin(thetaa);
ca = cos(alpha);
sa = sin(alpha);
T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
%end tmat

t01=T;
%(tmat)Equation 3.6 (Craig)
alpha=0;		%Note: alpha is in radians.
a=0;
d=dd2;
thetaa=0;		%Note: theta is in radians.
alpha = alpha*pi/180;    
thetaa = thetaa*pi/180;    
c = cos(thetaa);
s = sin(thetaa);
ca = cos(alpha);
sa = sin(alpha);
T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
%end tmat

t02=t01*T;
%(tmat)Equation 3.6 (Craig)
alpha=90;		%Note: alpha is in radians.
a=3.85;
d=dd3;
thetaa=0;		%Note: theta is in radians.
alpha = alpha*pi/180;    
thetaa = thetaa*pi/180;    
c = cos(thetaa);
s = sin(thetaa);
ca = cos(alpha);
sa = sin(alpha);
T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
%end tmat

t03=t02*T;
%(tmat)Equation 3.6 (Craig)
alpha=0;		%Note: alpha is in radians.
a=0;
d=3.75;
thetaa=0;		%Note: theta is in radians.
alpha = alpha*pi/180;    
thetaa = thetaa*pi/180;    
c = cos(thetaa);
s = sin(thetaa);
ca = cos(alpha);
sa = sin(alpha);
T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
%end tmat

t04=t03*T;
xtip(i)=t04(1,4);
ytip(i)=t04(2,4);
ztip(i)=t04(3,4);

end

%Equations for position of end link
xyz=[xtip' ytip' ztip'];
p15=[16.4*ones(size(theta1)),7.6*ones(size(theta1)),21.5*ones(size(theta1))];
magp15=sqrt((16.4)^2+(7.6)^2+21.5^2);
magxyztip=sqrt((xtip-3.85).^2+(ytip+3.75).^2+ztip.^2);
xyztip=[(xtip-3.85)' (ytip+3.75)' ztip'];

%Calculate the perpenducular distance between the actual line and interpolated one
for i=1:1:length(xtip);
alpha=acos((dot(p15(i,:),xyztip(i,:)))./(magxyztip(i)*magp15));
alpha2(i)=alpha;
end
%Error calculation
err=magxyztip.*sin(alpha2);
dis=magxyztip.*cos(alpha2);
%Setting up handle for error plot 
subplot(2,1,2)
ef=plot(0,0);
axis([0 30 0 .25]);
grid on
title('Path Error');
xlabel('Distance Along Trajectory (in.)');
ylabel('Distance from Trajectory (in.)');
subplot(2,1,1)
%add base wheel
[sx sy sz]=cylinder(7.12,50);
sz=-3*ones(size(sz))+sz;
surf(sx,sy,sz);

%animation in real time at 8 degree increments to speed up animation
figure(4)
for i=1:8:length(theta1);
    theta=theta1(i,:);
   dd2=d2(i,:);
   dd3=d3(i,:);
   
%(tmat)Equation 3.6 (Craig)
alpha=0;		%Note: alpha is in radians.
a=0;
d=0;
thetaa=theta;		%Note: theta is in radians.
alpha = alpha*pi/180;    
thetaa = thetaa*pi/180;    
c = cos(thetaa);
s = sin(thetaa);
ca = cos(alpha);
sa = sin(alpha);
T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
%end tmat

t01=T;

%(tmat)Equation 3.6 (Craig)
alpha=0;		%Note: alpha is in radians.
a=0;
d=dd2;
thetaa=0;		%Note: theta is in radians.
alpha = alpha*pi/180;    
thetaa = thetaa*pi/180;    
c = cos(thetaa);
s = sin(thetaa);
ca = cos(alpha);
sa = sin(alpha);
T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
%end tmat

t02=t01*T;

%(tmat)Equation 3.6 (Craig)
alpha=90;		%Note: alpha is in radians.
a=3.85;
d=dd3;
thetaa=0;		%Note: theta is in radians.
alpha = alpha*pi/180;    
thetaa = thetaa*pi/180;    
c = cos(thetaa);
s = sin(thetaa);
ca = cos(alpha);
sa = sin(alpha);
T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
%end tmat

t03=t02*T;

%(tmat)Equation 3.6 (Craig)
alpha=0;		%Note: alpha is in radians.
a=0;
d=3.75;
thetaa=0;		%Note: theta is in radians.
alpha = alpha*pi/180;    
thetaa = thetaa*pi/180;    
c = cos(thetaa);
s = sin(thetaa);
ca = cos(alpha);
sa = sin(alpha);
T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
%end tmat

t04=t03*T;

l1n=t01*l1;
l2n=t02*l2;
l3n=t03*l3;
%this is where the actual animation occurs
%animation of the robot
set(hdl1,'vertices',[l1n(1,:)' l1n(2,:)' l1n(3,:)']);
set(hdl2,'vertices',[l2n(1,:)' l2n(2,:)' l2n(3,:)']);
set(hdl3,'vertices',[l3n(1,:)' l3n(2,:)' l3n(3,:)']);
%animation of the interpolated position of end link
set(plt,'xdata',xyz(1:i,1),'ydata',xyz(1:i,2),'zdata',xyz(1:i,3));
%animation of the error of interpolated function
set(ef,'xdata',dis(:,1:i),'ydata',err(:,1:i));

drawnow
end
%Plot the actual line for trajectory
figure(4)
	hold on
	subplot(2,1,1)
	plot3(x,y,z,'g');
   
%end of program