build_up_skeleton.m

是一个在matlab下的机器人运动仿真的软件

function build_up_skeleton(hObject, envetdata, handles, varargin)

global face1base face2base face3base face4base face5base face6base...
       face1column1 face2column1 face3column1 face4column1 face5column1 face6column1...
       face1top face2top face3top face4top face5top face6top...
       face1small1 face2small1 face3small1 face4small1 face5small1 face6small1...
       face1column2 face2column2 face3column2 face4column2 face5column2 ...
       face1small2 face2small2 face3small2 face4small2 face5small2 face6small2...
       face1column3 face2column3 face3column3 face4column3 face5column3 face6column3...
       face1wrist face2wrist face3wrist face4wrist face5wrist face6wrist...
       face1cluth face2cluth face3cluth face4cluth face5cluth face6cluth...
       face1tool face2tool face3tool face4tool face5tool face6tool
   
global theta1 theta2 theta3 theta4 d1 d2 d3 l1 T06


axes(handles.axes2);
cla;
grid on; 
hold on;
axis([-300 300 -400 400 -30 350]);
%% Transformation matrix
%This is very important for the whole project. It is the algorithm for
%calculating movement.

T01=[cos(theta1),-sin(theta1),0,0;
    sin(theta1),cos(theta1),0,0;
    0,0,1,0;
    0,0,0,1];

T12=[1,0,0,0;
    0,1,0,0;
    0,0,1,d1;
    0,0,0,1];

T23=[1,0,0,l1;
    0,0,-1,-d2;
    0,1,0,0;
    0,0,0,1];

T34=[cos(theta2),-sin(theta2),0,0;
    sin(theta2),cos(theta2),0,0;
    0,0,1,d3;
    0,0,0,1];

T45=[cos(theta3),-sin(theta3),0,0;
    0,0,1,0;
    -sin(theta3),-cos(theta3),0,0;
    0,0,0,1];

T56=[cos(theta4),-sin(theta4),0,0;
    0,0,-1,0;
    sin(theta4),cos(theta4),0,0;
    0,0,0,1];

T06=T01*T12*T23*T34*T45*T56;

%% build up the base face
fill3(face1base(:,1),face1base(:,2),face1base(:,3),'g'); 
fill3(face2base(:,1),face2base(:,2),face2base(:,3),'g');
fill3(face3base(:,1),face3base(:,2),face3base(:,3),'g');
fill3(face4base(:,1),face4base(:,2),face4base(:,3),'g');
fill3(face5base(:,1),face5base(:,2),face5base(:,3),'g');
fill3(face6base(:,1),face6base(:,2),face6base(:,3),'g');

%% build up the column 1
   obj1column1=T01*transp(face1column1);
   obj2column1=T01*transp(face2column1);
   obj3column1=T01*transp(face3column1);
   obj4column1=T01*transp(face4column1);
   obj5column1=T01*transp(face5column1);
   obj6column1=T01*transp(face6column1);
   
   fill3(obj1column1(1,:),obj1column1(2,:),obj1column1(3,:),'b');
   fill3(obj2column1(1,:),obj2column1(2,:),obj2column1(3,:),'b');
   fill3(obj3column1(1,:),obj3column1(2,:),obj3column1(3,:),'b');
   fill3(obj4column1(1,:),obj4column1(2,:),obj4column1(3,:),'b');
   fill3(obj5column1(1,:),obj5column1(2,:),obj5column1(3,:),'b');
   fill3(obj6column1(1,:),obj6column1(2,:),obj6column1(3,:),'b');
   
%% build up the top column on column 1
   obj1top=T01*transp(face1top);
   obj2top=T01*transp(face2top);
   obj3top=T01*transp(face3top);
   obj4top=T01*transp(face4top);
   obj5top=T01*transp(face5top);
   obj6top=T01*transp(face6top);
   
   fill3(obj1top(1,:),obj1top(2,:),obj1top(3,:),'b');
   fill3(obj2top(1,:),obj2top(2,:),obj2top(3,:),'b');
   fill3(obj3top(1,:),obj3top(2,:),obj3top(3,:),'b');
   fill3(obj4top(1,:),obj4top(2,:),obj4top(3,:),'b');
   fill3(obj5top(1,:),obj5top(2,:),obj5top(3,:),'b');
   fill3(obj6top(1,:),obj6top(2,:),obj6top(3,:),'b');
   
%% build up the small part1 on column2
obj1small1=T01*T12*transp(face1small1);
obj2small1=T01*T12*transp(face2small1);
obj3small1=T01*T12*transp(face3small1);
obj4small1=T01*T12*transp(face4small1);
obj5small1=T01*T12*transp(face5small1);
obj6small1=T01*T12*transp(face6small1);

fill3(obj1small1(1,:),obj1small1(2,:),obj1small1(3,:),'y');
fill3(obj2small1(1,:),obj2small1(2,:),obj2small1(3,:),'y');
fill3(obj3small1(1,:),obj3small1(2,:),obj3small1(3,:),'y');
fill3(obj4small1(1,:),obj4small1(2,:),obj4small1(3,:),'y');
fill3(obj5small1(1,:),obj5small1(2,:),obj5small1(3,:),'y');
fill3(obj6small1(1,:),obj6small1(2,:),obj6small1(3,:),'y');

%% build up column2
obj1column2=T01*T12*transp(face1column2);
obj2column2=T01*T12*transp(face2column2);
obj3column2=T01*T12*transp(face3column2);
obj4column2=T01*T12*transp(face4column2);
obj5column2=T01*T12*transp(face5column2);

fill3(obj1column2(1,:),obj1column2(2,:),obj1column2(3,:),'y');
fill3(obj2column2(1,:),obj2column2(2,:),obj2column2(3,:),'y');
fill3(obj3column2(1,:),obj3column2(2,:),obj3column2(3,:),'y');
fill3(obj4column2(1,:),obj4column2(2,:),obj4column2(3,:),'y');
fill3(obj5column2(1,:),obj5column2(2,:),obj5column2(3,:),'y');

%% build up the small part2 on column2
obj1small2=T01*T12*transp(face1small2);
obj2small2=T01*T12*transp(face2small2);
obj3small2=T01*T12*transp(face3small2);
obj4small2=T01*T12*transp(face4small2);
obj5small2=T01*T12*transp(face5small2);
obj6small2=T01*T12*transp(face6small2);
   
fill3(obj1small2(1,:),obj1small2(2,:),obj1small2(3,:),'y');
fill3(obj2small2(1,:),obj2small2(2,:),obj2small2(3,:),'y');
fill3(obj3small2(1,:),obj3small2(2,:),obj3small2(3,:),'y');
fill3(obj4small2(1,:),obj4small2(2,:),obj4small2(3,:),'y');
fill3(obj5small2(1,:),obj5small2(2,:),obj5small2(3,:),'y');
fill3(obj6small2(1,:),obj6small2(2,:),obj6small2(3,:),'y');

%% build up the column 3
obj1column3=T01*T12*T23*transp(face1column3);
obj2column3=T01*T12*T23*transp(face2column3);
obj3column3=T01*T12*T23*transp(face3column3);
obj4column3=T01*T12*T23*transp(face4column3);
obj5column3=T01*T12*T23*transp(face5column3);
obj6column3=T01*T12*T23*transp(face6column3);

fill3(obj1column3(1,:),obj1column3(2,:),obj1column3(3,:),'r');
fill3(obj2column3(1,:),obj2column3(2,:),obj2column3(3,:),'r');
fill3(obj3column3(1,:),obj3column3(2,:),obj3column3(3,:),'r');
fill3(obj4column3(1,:),obj4column3(2,:),obj4column3(3,:),'r');
fill3(obj5column3(1,:),obj5column3(2,:),obj5column3(3,:),'r');
fill3(obj6column3(1,:),obj6column3(2,:),obj6column3(3,:),'r');

%% build up the wrist
 obj1wrist=T01*T12*T23*T34*transp(face1wrist);
 obj2wrist=T01*T12*T23*T34*transp(face2wrist);
 obj3wrist=T01*T12*T23*T34*transp(face3wrist);
 obj4wrist=T01*T12*T23*T34*transp(face4wrist);
 obj5wrist=T01*T12*T23*T34*transp(face5wrist);
 obj6wrist=T01*T12*T23*T34*transp(face6wrist);
     
fill3(obj1wrist(1,:),obj1wrist(2,:),obj1wrist(3,:),'g');
fill3(obj2wrist(1,:),obj2wrist(2,:),obj2wrist(3,:),'g');
fill3(obj3wrist(1,:),obj3wrist(2,:),obj3wrist(3,:),'g');
fill3(obj4wrist(1,:),obj4wrist(2,:),obj4wrist(3,:),'g');
fill3(obj5wrist(1,:),obj5wrist(2,:),obj5wrist(3,:),'g');
fill3(obj6wrist(1,:),obj6wrist(2,:),obj6wrist(3,:),'g');

%% build up the cluth
 obj1cluth=T01*T12*T23*T34*T45*transp(face1cluth);
 obj2cluth=T01*T12*T23*T34*T45*transp(face2cluth);
 obj3cluth=T01*T12*T23*T34*T45*transp(face3cluth);
 obj4cluth=T01*T12*T23*T34*T45*transp(face4cluth);
 obj5cluth=T01*T12*T23*T34*T45*transp(face5cluth);
 obj6cluth=T01*T12*T23*T34*T45*transp(face6cluth);  
     
fill3(obj1cluth(1,:),obj1cluth(2,:),obj1cluth(3,:),'b');
fill3(obj2cluth(1,:),obj2cluth(2,:),obj2cluth(3,:),'b');
fill3(obj3cluth(1,:),obj3cluth(2,:),obj3cluth(3,:),'b');
fill3(obj4cluth(1,:),obj4cluth(2,:),obj4cluth(3,:),'b');
fill3(obj5cluth(1,:),obj5cluth(2,:),obj5cluth(3,:),'b');
fill3(obj6cluth(1,:),obj6cluth(2,:),obj6cluth(3,:),'b');

%% build up the tool
 obj1tool=T01*T12*T23*T34*T45*T56*transp(face1tool);
 obj2tool=T01*T12*T23*T34*T45*T56*transp(face2tool);
 obj3tool=T01*T12*T23*T34*T45*T56*transp(face3tool);
 obj4tool=T01*T12*T23*T34*T45*T56*transp(face4tool);
 obj5tool=T01*T12*T23*T34*T45*T56*transp(face5tool);
 obj6tool=T01*T12*T23*T34*T45*T56*transp(face6tool);  
     

fill3(obj1tool(1,:),obj1tool(2,:),obj1tool(3,:),'c');
fill3(obj2tool(1,:),obj2tool(2,:),obj2tool(3,:),'c');
fill3(obj3tool(1,:),obj3tool(2,:),obj3tool(3,:),'c');
fill3(obj4tool(1,:),obj4tool(2,:),obj4tool(3,:),'c');
fill3(obj5tool(1,:),obj5tool(2,:),obj5tool(3,:),'c');
fill3(obj6tool(1,:),obj6tool(2,:),obj6tool(3,:),'c');