scm_cloos.m

针对移动机械臂运动学

%SCM_CLOOS Load kinematic data for a Cloos manipulator (Romat 56)
%Paper - Universidad Autonoma Queretaro - E. Martinez Ramirez, V. M. Hernandez Guzman, A. Zavala Rio,
%                                         C. G. Lopez Guevara y C. S. Lopez Cajun

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

syms L1 L2 L3 a1 a2 a3 a4 a5 a6 

%SCM joint parameters
%          Transl         Rot(RPY Angles)
%        X    Y    Z    A    B   C
joint1=[ 0    0    0   a1    0   0];%XYZ(0)A(a1)
joint2=[ 0    0    0    0  -a2   0];%XYZ(0)B(-a2)
joint3=[L1    0    0    0   a3   0];%X(L1)B(a3)
joint4=[ 0    0   L2   a4    0   0];%Z(L2)A(a4)
joint5=[ 0    0    0    0  -a5   0];%XYZ(0)B(-a5)
joint6=[ 0    0  -L3  -a6    0   0];%Z(-L3)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 pi];%XYZ(0)C(pi)
tool=               [0 0 0 0 0  0];%XYZ(0)ABC(0)
%===================================
ftr=[finalTransformation;tool];
%===================================
clear finalT* tool

%Auxiliary
variables=[a1 a2 a3 a4 a5 a6];
links=[L1 L2 L3
    .430 .430 .0667];%meters
%==========================================================================
chain=struct('variables',variables,'links',links(1,:),'linksValues',links(2,:));
%==========================================================================

%Description
description='Robot ''cloos'' ==> RRRRRR';

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

%Robot
%==========================================================================
cloos=struct('jointParameters',jp,'finalTransformation',ftr,'chain',chain,'description',description,'verifyData',verify);
%==========================================================================
manipulator=cloos;
%==================
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