robodemo.rar

  • hammerboy99
    了解作者
  • matlab
    开发工具
  • 3KB
    文件大小
  • rar
    文件格式
  • 0
    收藏次数
  • 1 积分
    下载积分
  • 4
    下载次数
  • 2010-04-12 12:30
    上传日期
this is very simple robot demo program using matlab and you can take a graphical output
robodemo.rar
  • robodemo.m
    10.4KB
内容介绍
%%%%%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 pineza@wwc.edu % Adam Hansel hansad@wwc.edu % 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)' (yt
评论
    相关推荐
    • jqGridDemo
      官网也有的(http://blog.mn886.net/jqGrid/ )上传文件是根据官网的demo调试的。方便自己查阅
    • SQLite demo
      SQLite demo, DBUtil demo, commont dbcp demo. 初学者参考
    • angularjsdemo
      angularjsdemo 一个简单的应用演示,展示了游戏部门网站上出售的一些游戏。 主要使用 AngularJS。
    • mschart demo
      国外网站下的一个DEMO,包括了饼图、曲线、柱状图的2、3D绘制,不要再下那些简陋的不足以遮羞的DEMO
    • demo
      演示 RabbitMQ演示 卡夫卡演示 叉加入演示
    • jqueryDemo
      初学jquery的非常不错的demo (描述重点就可以了啊。还要有字数的限制。。 初学jquery的非常不错的demo 初学jquery的非常不错的demo 初学jquery的非常不错的demo )
    • quartzdemo
      C#下的定时任务demo,可以实现很多后台的服务,和自己的系统分开很方便,这是一个demo
    • demo
      git-demo
    • Demo DELTA.zip
      Demo Delta for Industry
    • demo.rar
      图形化demo,直观效果明显,图形化演示demo,VISIONBOX