[base_vertices_in_I, base_faces, ~, ~, ~] = stlread('Denso_6242_Base.STL');
[link1_vertices_in_1, link1_faces, ~, ~, ~] = stlread('Denso_6242_Link1.STL');
[link2_vertices_in_2, link2_faces, ~, ~, ~] = stlread('Denso_6242_Link2.STL');
[link3_vertices_in_3, link3_faces, ~, ~, ~] = stlread('Denso_6242_Link3.STL');
[link4_vertices_in_4, link4_faces, ~, ~, ~] = stlread('Denso_6242_Link4.STL');
[link5_vertices_in_5, link5_faces, ~, ~, ~] = stlread('Denso_6242_Link5.STL');
[gripper_vertices_in_E, gripper_faces, ~, ~, ~] = stlread('VP_6242_Gripper_JawE.STL');
IT1 = rotz(gamma(1));
oneT2 = roty(gamma(2));
twoT3 = roty(gamma(3));
threeT4 = roty(-pi/2)*rotx(gamma(4));
fourT5 = roty(gamma(5));
fiveTE = rotx(gamma(6));
IIr1 = [0;0;155];
oneoner2 = [0;0;123.5];
twotwor3 = [0;0;210];
threethreer4 = [-75;0;86];
fourfourr5 = [125.0;0;0];
fivefiverE = [132.5;0;0];
IT2 = IT1*oneT2;
IT3 = IT2*twoT3;
IT4 = IT3*threeT4;
IT5 = IT4*fourT5;
ITE = IT5*fiveTE
IIr2 = IIr1 + IT1*oneoner2;
IIr3 = IIr2 + IT2*twotwor3;
IIr4 = IIr3 + IT3*threethreer4;
IIr5 = IIr4 + IT4*fourfourr5;
IIrE = IIr5 + IT5*fivefiverE
IqE=quatmult([cos(gamma(1)/2);0;0;sin(gamma(1)/2)],[cos(gamma(2)/2);0;sin(gamma(2)/2);0]);
IqE=quatmult(IqE,[cos(gamma(3)/2);0;sin(gamma(3)/2);0]);
IqE=quatmult(IqE,[cos(gamma(4)/2);sin(gamma(4)/2);0;0]);
IqE=quatmult(IqE,[cos(gamma(5)/2);0;sin(gamma(5)/2);0]);
IqE=quatmult(IqE,[cos(gamma(6)/2);sin(gamma(6)/2);0;0]);
link1_vertices_in_I=IIr1+IT1*link1_vertices_in_1';
link2_vertices_in_I=IIr2+IT2*link2_vertices_in_2';
link3_vertices_in_I=IIr3+IT3*link3_vertices_in_3';
link4_vertices_in_I=IIr4+IT4*link4_vertices_in_4';
link5_vertices_in_I=IIr5+IT5*link5_vertices_in_5';
gripper_vertices_in_I=IIrE+ITE*gripper_vertices_in_E';
p1=patch('Faces',base_faces,'Vertices',base_vertices_in_I,'EdgeColor','None','FaceColor',[0 0 1]);
p2=patch('Faces',link1_faces,'Vertices',link1_vertices_in_I','EdgeColor','None','FaceColor',[1 0 1]);
p3=patch('Faces',link2_faces,'Vertices',link2_vertices_in_I','EdgeColor','None','FaceColor',[1 0 1]);
p4=patch('Faces',link3_faces,'Vertices',link3_vertices_in_I','EdgeColor','None','FaceColor',[1 0 1]);
p5=patch('Faces',link4_faces,'Vertices',link4_vertices_in_I','EdgeColor','None','FaceColor',[1 0 1]);
p6=patch('Faces',link5_faces,'Vertices',link5_vertices_in_I','EdgeColor','None','FaceColor',[1 0 1]);
p7=patch('Faces',gripper_faces,'Vertices',gripper_vertices_in_I','EdgeColor','None','FaceColor',[1 0 0]);
axis equal
camlight left
set(gca,'projection','perspective')
view([1;1;.5])
xlabel('x')
ylabel('y')
zlabel('z')