matlab机器人工具箱matlabrobotics_toolbox_demo

MATLABROBOTTOOLrtdemo演示目录一、rtdemo机器人工具箱演示..............................................2二、transfermations坐标转换.................................................2三、Trajectory齐次方程..........................................................8四、forwardkinematics运动学正解.....................................12四、Animation动画..............................................................18五、InverseKinematics运动学逆解......................................23六、Jacobians雅可比---矩阵与速度...................................32七、InverseDynamics逆向动力学........................................45八、ForwardDynamics正向动力学.......................................52一、rtdemo机器人工具箱演示rtdemo%二、transfermations坐标转换%Inthefieldofroboticstherearemanypossiblewaysofrepresenting%positionsandorientations,butthehomogeneoustransformationiswell%matchedtoMATLABspowerfultoolsformatrixmanipulation.%%HomogeneoustransformationsdescribetherelationshipsbetweenCartesian%coordinateframesintermsoftranslationandorientation.%Apuretranslationof0.5mintheXdirectionisrepresentedbytransl(0.5,0.0,0.0)ans=1.0000000.500001.000000001.000000001.0000%%arotationof90degreesabouttheYaxisbyroty(pi/2)ans=0.000001.0000001.000000-1.000000.000000001.0000%%andarotationof-90degreesabouttheZaxisbyrotz(-pi/2)ans=0.00001.000000-1.00000.000000001.000000001.0000%%thesemaybeconcatenatedbymultiplicationt=transl(0.5,0.0,0.0)*roty(pi/2)*rotz(-pi/2)t=0.00000.00001.00000.5000-1.00000.000000-0.0000-1.00000.000000001.0000%%Ifthistransformationrepresentedtheoriginofanewcoordinateframewithrespect%totheworldframeorigin(0,0,0),thatneworiginwouldbegivenbyt*[0001]'ans=0.5000001.0000pause%anykeytocontinue%%theorientationofthenewcoordinateframemaybeexpressedintermsof%Euleranglestr2eul(t)ans=01.5708-1.5708%%orroll/pitch/yawanglestr2rpy(t)ans=-1.57080.0000-1.5708pause%anykeytocontinue%%Itisimportanttonotethattranformmultiplicationisingeneralnot%commutativeasshownbythefollowingexamplerotx(pi/2)*rotz(-pi/8)ans=0.92390.382700-0.00000.0000-1.00000-0.38270.92390.000000001.0000rotz(-pi/8)*rotx(pi/2)ans=0.92390.0000-0.38270-0.38270.0000-0.9239001.00000.000000001.0000%%pause%anykeytocontinueechooff三、Trajectory齐次方程%Thepathwillmovetherobotfromitszeroangleposetotheupright(or%READY)pose.%%Firstcreateatimevector,completingthemotionin2secondswitha%sampleintervalof56ms.t=[0:.056:2];pause%hitanykeytocontinue%%Apolynomialtrajectorybetweenthe2posesiscomputedusingjtraj()%q=jtraj(qz,qr,t);pause%hitanykeytocontinue%%Forthisparticulartrajectorymostofthemotionisdonebyjoints2and3,%andthiscanbeconvenientlyplottedusingstandardMATLABoperationssubplot(2,1,1)plot(t,q(:,2))title('Theta')xlabel('Time(s)');ylabel('Joint2(rad)')subplot(2,1,2)plot(t,q(:,3))xlabel('Time(s)');ylabel('Joint3(rad)')pause%hitanykeytocontinue%%Wecanalsolookatthevelocityandaccelerationprofiles.Wecould%differentiatetheangletrajectoryusingdiff(),butmoreaccurateresults%canbeobtainedbyrequestingthatjtraj()returnangularvelocityand%accelerationasfollows[q,qd,qdd]=jtraj(qz,qr,t);%%whichcanthenbeplottedasbeforesubplot(2,1,1)plot(t,qd(:,2))title('Velocity')xlabel('Time(s)');ylabel('Joint2vel(rad/s)')subplot(2,1,2)plot(t,qd(:,3))xlabel('Time(s)');ylabel('Joint3vel(rad/s)')pause(2)%andthejointaccelerationprofilessubplot(2,1,1)plot(t,qdd(:,2))title('Acceleration')xlabel('Time(s)');ylabel('Joint2accel(rad/s2)')subplot(2,1,2)plot(t,qdd(:,3))xlabel('Time(s)');ylabel('Joint3accel(rad/s2)')pause%anykeytocontinueechooff四、forwardkinematics运动学正解%ForwardkinematicsistheproblemofsolvingtheCartesianpositionand%orientationofamechanismgivenknowledgeofthekinematicstructureand%thejointcoordinates.%%ConsiderthePuma560exampleagain,andthejointcoordinatesofzero,%whicharedefinedbyqzqzqz=000000%%Theforwardkinematicsmaybecomputedusingfkine()withanappropropriate%kinematicdescription,inthiscase,thematrixp560whichdefines%kinematicsforthe6-axisPuma560.fkine(p560,qz)ans=1.0000000.452101.00000-0.1500001.00000.43180001.0000%%returnsthehomogeneoustransformcorrespondingtothelastlinkofthe%manipulatorpause%anykeytocontinue%%fkine()canalsobeusedwithatimesequenceofjointcoordinates,or%trajectory,whichisgeneratedbyjtraj()%t=[0:.056:2];%generateatimevectorq=jtraj(qz,qr,t);%computethejointcoordinatetrajectory%%thenthehomogeneoustransformforeachsetofjointcoordinatesisgivenbyT=fkine(p560,q);%%whereTisa3-dimensionalmatrix,thefirsttwodimensionsarea4x4%homogeneoustransformationandthethirddimensionistime.%%Forexample,thefirstpointisT(:,:,1)ans=1.0000000.452101.00000-0.1500001.00000.43180001.0000%%andthetenthpointisT(:,:,10)ans=1.0000-0.000000.4455-0.00001.00000-0.1500001.00000.50680001.0000pause%anykeytocontinue%%Elements(1:3,4)correspondtotheX,YandZcoordinatesrespectively,and%maybeplottedagainsttimesubplot(3,1,1)plot(t,squeeze(T(1,4,:)))xlabel('Time(s)');ylabel('X(m)')subplot(3,1,2)plot(t,squeeze(T(2,4,:)))xlabel('Time(s)');ylabel('Y(m)')subplot(3,1,3)plot(t,squeeze(T(3,4,:)))xlabel('Time(s)');ylabel('Z(m)')pause%anykeytocontinue%%orwecouldplotXagainstZtogetsomeideaoftheCartesianpathfollowed%bythemanipulator.%subplot(1,1,1)plot(squeeze(T