MATLAB.zip_Want It_matlab rotation_matlab rotation

function [c1,c2,c3,c4,r] = robot_arm_rotation_main %rotation of robot arm %begin of initialisation of arm elements close all clc clear bid_vrtcs = [-2 -2 -2 -2 2 -2 2 2 -2 2 -2 -2 -2 -2 2 -2 2 2 2 2 2 2 -2 2]; bid_fcs = [1 2 3 4 1 5 6 2 2 6 7 3 3 7 8 4 6 8 5 1 5 8 7 6]; arm_elmnt_vrtcs = [ -5 -5 -12 -5 5 -12 -5 5 12 -5 -5 12 5 5 -12 5 -5 -12 5 -5 12 5 5 12 -4 5 11 4 5 11 -4 5 -11 4 5 -11 -4 -5 -11 4 -5 -11 -4 -5 11 4 -5 11 ]; arm_elmnt_faces = [ 1 2 3 4 4 3 8 7 7 8 5 6 1 2 5 6 13 11 12 14 14 12 10 16 9 10 16 15 9 15 13 11 2 3 9 11 3 9 10 8 10 12 5 8 2 11 12 5 1 4 15 13 4 15 16 7 7 6 14 16 1 13 14 6 ]; arm_elmnt_1_current_vector = [0 0 1]; arm_elmnt_2_current_vector = [0 0 1]; arm_elmnt_3_current_vector = [0 0 1]; arm_elmnt_4_current_vector = [0 1 0]; arm_elmnt_2_center = [0 0 18]; arm_elmnt_3_center = [18 0 18]; arm_elmnt_4_center = [18 18 18]; arm_elmnt_vrtcs_1_dir = arm_elmnt_vrtcs; arm_elmnt_vrtcs_2_dir = set_new_direction(arm_elmnt_vrtcs,arm_elmnt_2_current_vector,[1 0 0]); arm_elmnt_vrtcs_3_dir = set_new_direction(arm_elmnt_vrtcs,arm_elmnt_3_current_vector,[0 1 0]); arm_elmnt_vrtcs_4_dir = set_new_direction(set_new_direction(arm_elmnt_vrtcs,arm_elmnt_4_current_vector,[1 0 0]),[0 0 1],[1 0 0]); arm_elmnt_1_current_vector = [ 0 0 1 ]; arm_elmnt_2_current_vector = [ 1 0 0 ]; arm_elmnt_3_current_vector = [ 0 1 0 ]; arm_elmnt_4_current_vector = [ 1 0 0 ]; % bid_1_axis_x = arm_elmnt_2_current_vector; % bid_1_axis_y = [ 0 1 0]; % bid_1_axis_z = [ 0 0 1]; % % bid_2_axis_x = arm_elmnt_3_current_vector; % bid_2_axis_y = [-1 0 0]; % bid_2_axis_z = [ 0 0 1]; % % bid_3_axis_x = arm_elmnt_4_current_vector; % bid_3_axis_y = [ 0 -1 0]; % bid_3_axis_z = [ 0 0 1]; % bid_1_center = [0 0 25]; % bid_2_center = [25 0 18.5]; % bid_3_center = [18.5 25 18.5]; bid_1_center = [0 0 0]; bid_2_center = [0 0 0]; bid_3_center = [0 0 0]; arm_elmnt_vrtcs_1 = arm_elmnt_vrtcs_1_dir; arm_elmnt_vrtcs_2 = move_to(arm_elmnt_vrtcs_2_dir, arm_elmnt_2_center); arm_elmnt_vrtcs_3 = move_to(arm_elmnt_vrtcs_3_dir, arm_elmnt_3_center); arm_elmnt_vrtcs_4 = move_to(arm_elmnt_vrtcs_4_dir, arm_elmnt_4_center); bid_1_vrtcs = move_to(bid_vrtcs, bid_1_center); bid_2_vrtcs = move_to(bid_vrtcs, bid_2_center); bid_3_vrtcs = move_to(bid_vrtcs, bid_3_center); arm_elmnt_1 = patch('Vertices',arm_elmnt_vrtcs_1,'Faces',arm_elmnt_faces,'FaceColor','red'); arm_elmnt_2 = patch('Vertices',arm_elmnt_vrtcs_2,'Faces',arm_elmnt_faces,'FaceColor','green'); arm_elmnt_3 = patch('Vertices',arm_elmnt_vrtcs_3,'Faces',arm_elmnt_faces,'FaceColor','yellow'); arm_elmnt_4 = patch('Vertices',arm_elmnt_vrtcs_4,'Faces',arm_elmnt_faces,'FaceColor','cyan'); % arm_elmnt_5 = patch('Vertices',arm_elmnt_vrtcs_1,'Faces',arm_elmnt_faces,'FaceColor','red'); % arm_elmnt_6 = patch('Vertices',arm_elmnt_vrtcs_2,'Faces',arm_elmnt_faces,'FaceColor','green'); % arm_elmnt_7 = patch('Vertices',arm_elmnt_vrtcs_3,'Faces',arm_elmnt_faces,'FaceColor','yellow'); % arm_elmnt_8 = patch('Vertices',arm_elmnt_vrtcs_4,'Faces',arm_elmnt_faces,'FaceColor','cyan'); bid_1 = patch('Vertices',bid_1_vrtcs,'Faces',bid_fcs,'FaceColor','red'); bid_2 = patch('Vertices',bid_2_vrtcs,'Faces',bid_fcs,'FaceColor','red'); bid_3 = patch('Vertices',bid_3_vrtcs,'Faces',bid_fcs,'FaceColor','red'); %initialization ends grid; %view(-90,0); % axis([-0.5 1.5 -0.5 1.5 -0.5 1.5]); axis([-40 40 -40 40 -40 40]); xlabel('X'); ylabel('Y'); zlabel('Z'); a=[]; a1=[]; a2=[]; a3=[]; p=[]; hold all for i=0:0.05:20*pi angles = [i*2/3+sin(rand)/100, -i/3+sin(rand)/100, i+sin(rand)/100]; % angles = [0, 0, 0]; %angles = [0.3, 0.1, 0.5]; %arm_elmnt_1_current_vector = [ 0 0 1 ]; %calculates DCM and currend dirrecton for arm element rotation DCM1=Rotation(angles(1),arm_elmnt_1_current_vector); arm_elmnt_vrtcs_2=arm_elmnt_vrtcs_2_dir*DCM1; arm_elmnt_2_current_vector1 = arm_elmnt_2_current_vector*DCM1; bid_1_vrtcs_new=bid_1_vrtcs*DCM1; DCM2=Rotation(angles(2),arm_elmnt_2_current_vector1); arm_elmnt_vrtcs_3=arm_elmnt_vrtcs_3_dir*DCM1*DCM2; arm_elmnt_3_current_vector1 = arm_elmnt_3_current_vector*DCM1*DCM2; bid_2_vrtcs_new=bid_2_vrtcs*DCM1*DCM2; DCM3=Rotation(angles(3),arm_elmnt_3_current_vector1); arm_elmnt_vrtcs_4=arm_elmnt_vrtcs_4_dir*DCM1*DCM2*DCM3; arm_elmnt_4_current_vector1 = arm_elmnt_4_current_vector*DCM1*DCM2*DCM3; bid_3_vrtcs_new=bid_3_vrtcs*DCM1*DCM2*DCM3; % disp(arm_elmnt_2_current_vector1); % disp(arm_elmnt_3_current_vector1); % disp(arm_elmnt_4_current_vector1); % disp(DCM1); % disp(DCM2); % disp(DCM3); %calculating arm elements center movement delta2=[0 0 18]; delta3=[18 0 0]*DCM1; delta4=[0 18 0]*DCM1*DCM2; arm_elmnt_2_center=delta2; %disp(arm_elmnt_2_center) arm_elmnt_3_center=delta3+delta2; arm_elmnt_4_center=delta4+delta3+delta2;% disp(arm_elmnt_4_center) %calculating arm elements center movement del2=[0 0 25]; del3=[25 0 0]*DCM1; del4=[0 25 0]*DCM1*DCM2; bid_1_center=del2; %disp(arm_elmnt_2_center) bid_2_center=del3+[0 0 18.5]; bid_3_center=del4+[18.5 0 0]*DCM1 + [0 0 18.5];% disp(arm_elmnt_4_center) bid_1_axes=[nor(arm_elmnt_vrtcs_2(1,:)-arm_elmnt_vrtcs_2(2,:));... nor(arm_elmnt_vrtcs_2(5,:)-arm_elmnt_vrtcs_2(2,:));... nor(arm_elmnt_vrtcs_2(3,:)-arm_elmnt_vrtcs_2(2,:))] ; % check that (test mode) bid_2_axes=[nor(arm_elmnt_vrtcs_3(1,:)-arm_elmnt_vrtcs_3(2,:));... nor(arm_elmnt_vrtcs_3(5,:)-arm_elmnt_vrtcs_3(2,:));... nor(arm_elmnt_vrtcs_3(3,:)-arm_elmnt_vrtcs_3(2,:))] ; bid_3_axes=[nor(arm_elmnt_vrtcs_4(1,:)-arm_elmnt_vrtcs_4(2,:));... nor(arm_elmnt_vrtcs_4(5,:)-arm_elmnt_vrtcs_4(2,:));... nor(arm_elmnt_vrtcs_4(3,:)-arm_elmnt_vrtcs_4(2,:))] ; %here comes part of bid axes transformation to DCM %here comes part of bid DCM trasformation to angles bid_1_vrtcs_new = move_to(bid_1_vrtcs_new, bid_1_center); bid_2_vrtcs_new = move_to(bid_2_vrtcs_new, bid_2_center); bid_3_vrtcs_new = move_to(bid_3_vrtcs_new, bid_3_center); set(bid_1,'Vertices',bid_1_vrtcs_new,'Faces',bid_fcs); set(bid_2,'Vertices',bid_2_vrtcs_new,'Faces',bid_fcs); set(bid_3,'Vertices',bid_3_vrtcs_new,'Faces',bid_fcs); arm_elmnt_vrtcs_2 = move_to(arm_elmnt_vrtcs_2, arm_elmnt_2_center); arm_elmnt_vrtcs_3 = move_to(arm_elmnt_vrtcs_3, arm_elmnt_3_center); arm_elmnt_vrtcs_4 = move_to(arm_elmnt_vrtcs_4, arm_elmnt_4_center); set(arm_elmnt_1,'Vertices',arm_elmnt_vrtcs_1,'Faces',arm_elmnt_faces); set(arm_elmnt_2,'Vertices',arm_elmnt_vrtcs_2,'Faces',arm_elmnt_faces); set(arm_elmnt_3,'Vertices',arm_elmnt_vrtcs_3,'Faces',arm_elmnt_faces); set(arm_elmnt_4,'Vertices',arm_elmnt_vrtcs_4,'Faces',arm_elmnt_faces); % disp('----') % disp(angles) % [pitch, roll, yaw] = dcm2angle(DCM3'*DCM2'*DCM1','YXZ'); % % [pitch, roll, yaw] = dcm2angle(DCM1*DCM2*DCM3,'ZXY'); % disp( [-pitch, -roll, -yaw]); % disp('----') a=[a; bid_1_center]; a1=[a1; bid_2_center]; a2=[a2; bid_3_center]; drawnow; pause(.01); dcm = [-bid_3_axes(3,:); bid_3_axes(2,:); bid_3_axes(1,:)]; p = [p ;angles]; end plot3([0,bid_3_axes(1,1)],[0,bid_3_axes(1,2)]