scm_stanford.m

针对移动机械臂运动学

%SCM_STANFORD Load kinematic data for a Stanford manipulator
%FUNDAMENTOS DE ROBOTICA - Versao2.02 - pag 61,62 - (Alberto Adade Filho)

%July ~ December 2005, by Reinaldo M. do Nascimento

syms L0 L1 L2 a1 a2 d3 a4 a5 a6

%SCM joint parameters
%         Transl      Rot(RPY Angles)
%        X  Y   Z    A   B   C
joint1=[ 0  0  L0   a1   0   0];%Z(L0)A(a1)
joint2=[ 0 L1   0    0  a2   0];%Y(L1)B(a2)
joint3=[ 0  0  d3    0   0   0];%Z(d3)ABC(0)
joint4=[ 0  0   0   a4   0   0];%XYZ(0)A(a4)
joint5=[ 0  0   0    0  a5   0];%XYZ(0)B(a5)
joint6=[ 0  0  L2   a6   0   0];%Z(L2)A(a6)
%=================================================
jp=[joint1;joint2;joint3;joint4;joint5;joint6];
%=================================================
clear joint*

%Final Transformation
%                    X Y Z A B C
finalTransformation=[0 0 0 0 0 0];%XYZ(0)ABC(0)
tool=               [0 0 0 0 0 0];%XYZ(0)ABC(0)
%===================================
ftr=[finalTransformation;tool];
%===================================
clear finalT* tool

%Auxiliary
variables=[a1 a2 d3 a4 a5 a6];
links=[L0 L1 L2
     .412 .154 .263];%meters (only for test purposes)
%==========================================================================
chain=struct('variables',variables,'links',links(1,:),'linksValues',links(2,:));
%==========================================================================

%Description
description='Robot ''stanford'' ==> RRPRRR';

%Debug (1==on/0==off)
verify=1;

%Robot
%==========================================================================
stanford=struct('jointParameters',jp,'finalTransformation',ftr,'chain',chain,'description',description,'verifyData',verify);
%==========================================================================
manipulator=stanford;
%=====================
clear variables links chain

%Display Visualization
disp('=========================================================');
disp(description);
disp('=========================================================');
disp('SCM Joint Parameters');disp('   X   Y   Z   A   B   C');disp(jp);
disp('Final Transformation ');disp(ftr(1,:));
disp('Tool');disp(ftr(2,:));
disp('Verify Data');disp(verify);

clear jp ftr description verify