%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% ///--- Main Program of the Reverse Kinematics ---\\\ %% %% %% %% // Designed by: Marco Eckert 21/07/2011 %% %% %% %% // Version 1.23 %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear all; % Clear all variables in workspace clc; % Clear command window % Request m-File Parameter_Inverse_Kinematics Parameter_Inverse_Kinematics fprintf ('\n Geometry defined --> To calculate please press any key...\n'); pause; format long; choice = 1; p = 9; q = 0; while choice ~= 3 fprintf ('\n\n ...::: Parameter Calculation Inverse Kinematics :::... \n\n'); fprintf (' Press 1 --> Stroke Calculation by Line Movement of TCP\n'); % fprintf (' Press 2 --> Max/Min Stroke\n'); fprintf (' Press 3 --> Quit\n'); choice = input ('\n Please enter the Paramter you want to calculate: '); switch choice case 1 % Calculation of the Stroke: p = 9 + q; TCP_pos = input ('\n Please enter the TCP coordinates [x; y; z] in mm: '); TCP_speed = input ('\n Please enter the TCP speed [x; y; z] in mm/s: '); TCP_acc = input ('\n Please enter the TCP acceleration [x; y; z] in mm/sē: '); TCP(p) = {TCP_pos}; % Request m-File Leg_Calculation_Inverse_Kinematics Leg_Calculation_Inverse_Kinematics % Calculation of the travelling distances: r_11 = TCP {p} + ed_1 - leg_1 - bc_1; r_21 = TCP {p} + ed_2 - leg_2 - bc_2; r_31 = TCP {p} + ed_3 - leg_3 - bc_3; % Calculation of the stroke speeds with Jacobian Matrix: unitvect_p1 = [(leg_1(1)/ sqrt(leg_1(1)^2 + leg_1(2)^2 + leg_1(3)^2)); (leg_1(2)/ sqrt(leg_1(1)^2 + leg_1(2)^2 + leg_1(3)^2)); (leg_1(3)/ sqrt(leg_1(1)^2 + leg_1(2)^2 + leg_1(3)^2))]; unitvect_p2 = [(leg_2(1)/ sqrt(leg_2(1)^2 + leg_2(2)^2 + leg_2(3)^2)); (leg_2(2)/ sqrt(leg_2(1)^2 + leg_2(2)^2 + leg_2(3)^2)); (leg_2(3)/ sqrt(leg_2(1)^2 + leg_2(2)^2 + leg_2(3)^2))]; unitvect_p3 = [(leg_3(1)/ sqrt(leg_3(1)^2 + leg_3(2)^2 + leg_3(3)^2)); (leg_3(2)/ sqrt(leg_3(1)^2 + leg_3(2)^2 + leg_3(3)^2)); (leg_3(3)/ sqrt(leg_3(1)^2 + leg_3(2)^2 + leg_3(3)^2))]; J = [(unitvect_p1'/ (u_1' * unitvect_p1));... (unitvect_p2'/ (u_2' * unitvect_p2));... (unitvect_p3'/ (u_3' * unitvect_p3))]; s_speed = J * TCP_speed; % Calculation of the stroke accelerations: w_1 = ((cross(TCP_speed, unitvect_p1) - s_speed(1,1) * cross(u_1, unitvect_p1)))/ l(1,1); w_2 = ((cross(TCP_speed, unitvect_p2) - s_speed(2,1) * cross(u_2, unitvect_p2)))/ l(2,1); w_3 = ((cross(TCP_speed, unitvect_p3) - s_speed(3,1) * cross(u_3, unitvect_p3)))/ l(3,1); s_acc = J * TCP_acc + [ (l(1,1) * (w_1(1,1)^2 + w_1(2,1)^2 + w_1(3,1)^2))/ (u_1' * unitvect_p1);... (l(2,1) * (w_2(1,1)^2 + w_2(2,1)^2 + w_2(3,1)^2))/ (u_2' * unitvect_p2);... (l(3,1) * (w_3(1,1)^2 + w_3(2,1)^2 + w_3(3,1)^2))/ (u_3' * unitvect_p1)]; %---------------------------------------------------- % Calculation Transformation-Matrix T_RB_R1,2,3: x0 = [0.5, 0.1, 0.5, 0, 0.2, 0.01]; leg_v1 = TCP {p} - r_11 - bc_1 + ed_1; [x] = fsolve(@(x)Calc_Trans_RB_R1(x, leg_v1, l_1) ,x0); Phi_1 = atan2(x(1),x(4)); Theta_1 = atan2(x(2),x(5)); Psi_1 = atan2(x(3),x(6)); p_1 = [ cos(Psi_1) * cos(Phi_1) - sin(Psi_1) * cos(Theta_1) * sin(Phi_1);... -cos(Psi_1) * sin(Phi_1) - sin(Psi_1) * cos(Theta_1) * cos(Phi_1);... sin(Psi_1) * sin(Theta_1)]; q_1 = [ sin(Psi_1) * cos(Phi_1) + cos(Psi_1) * cos(Theta_1) * sin(Phi_1);... -sin(Psi_1) * sin(Phi_1) + cos(Psi_1) * cos(Theta_1) * cos(Phi_1);... -cos(Psi_1) * sin(Theta_1)]; r_1 = [ sin(Theta_1) * sin(Phi_1);... sin(Theta_1) * cos(Phi_1);... cos(Theta_1)]; T_RB_R1 = [ p_1(1,1), q_1(1,1), r_1(1,1);... p_1(2,1), q_1(2,1), r_1(2,1);... p_1(3,1), q_1(3,1), r_1(3,1)]; x0 = [0.5, 0.1, 0.5, 0, 0.2, 0.01]; leg_v2 = TCP_pos - r_21 - bc_2 + ed_2; [x] = fsolve(@(x)Calc_Trans_RB_R2(x, leg_v2, l_2) ,x0); Phi_2 = atan2(x(1),x(4)); Theta_2 = atan2(x(2),x(5)); Psi_2 = atan2(x(3),x(6)); p_2 = [ cos(Psi_1) * cos(Phi_1) - sin(Psi_1) * cos(Theta_1) * sin(Phi_1);... -cos(Psi_1) * sin(Phi_1) - sin(Psi_1) * cos(Theta_1) * cos(Phi_1);... sin(Psi_1) * sin(Theta_1)]; q_2 = [ sin(Psi_1) * cos(Phi_1) + cos(Psi_1) * cos(Theta_1) * sin(Phi_1);... -sin(Psi_1) * sin(Phi_1) + cos(Psi_1) * cos(Theta_1) * cos(Phi_1);... -cos(Psi_1) * sin(Theta_1)]; r_2 = [ sin(Theta_1) * sin(Phi_1);... sin(Theta_1) * cos(Phi_1);... cos(Theta_1)]; T_RB_R2 = [ p_2(1,1), q_2(1,1), r_2(1,1);... p_2(2,1), q_2(2,1), r_2(2,1);... p_2(3,1), q_2(3,1), r_2(3,1)]; x0 = [0.5, 0.1, 0.5, 0, 0.2, 0.01]; leg_v3 = TCP_pos - r_31 - bc_3 + ed_3; [x] = fsolve(@(x)Calc_Trans_RB_R3(x, leg_v3, l_3) ,x0); Phi_3 = atan2(x(1),x(4)); Theta_3 = atan2(x(2),x(5)); Psi_3 = atan2(x(3),x(6)); p_3 = [ cos(Psi_1) * cos(Phi_1) - sin(Psi_1) * cos(Theta_1) * sin(Phi_1);... -cos(Psi_1) * sin(Phi_1) - sin(Psi_1) * cos(Theta_1) * cos(Phi_1);... sin(Psi_1) * sin(Theta_1)]; q_3 = [ sin(Psi_1) * cos(Phi_1) + cos(Psi_1) * cos(Theta_1) * sin(Phi_1);... -sin(Psi_1) * sin(Phi_1) + cos(Psi_1) * cos(Theta_1) * cos(Phi_1);... -cos(Psi_1) * sin(Theta_1)]; r_3 = [ sin(Theta_1) * sin(Phi_1);... sin(Theta_1) * cos(Phi_1);... cos(Theta_1)]; T_RB_R3 = [ p_3(1,1), q_3(1,1), r_3(1,1);... p_3(2,1), q_3(2,1), r_3(2,1);... p_3(3,1), q_3(3,1), r_3(3,1)]; %---------------------------------------------------- %---------------------------------------------------- % Check of the strokes via Direct Kinematics: % Request m-File Leg_Calculation_Direct_Kinematics Leg_Calculation_Direct_Kinematics %---------------------------------------------------- % Display of the calculation results: fprintf ('\n TCP: [%g; %g; %g] \n', TCP{p}(1,1), TCP{p}(2,1), TCP{p}(3,1)); fprintf ('\n Stroke s1: %g mm \n', r_11(2,1)); fprintf (' Storke s2: %g mm \n', r_21(2,1)); fprintf (' Stroke s3: %g mm \n', r_31(2,1)); fprintf ('\n Speed s1: %g mm/s \n', s_speed(1,1)); fprintf (' Speed s2: %g mm/s \n', s_speed(2,1)); fprintf (' Speed s3: %g mm/s \n', s_speed(3,1)); fprintf ('\n Acceleration s1: %g mm/sē \n', s_acc(1)); fprintf (' Acceleration s2: %g mm/sē \n', s_acc(2)); fprintf (' Acceleration s3: %g mm/sē \n', s_acc(3)); fprintf ('\n Direct Kinematics TCP: [%g; %g; %g] \n', r_p(1,1), r_p(2,1), r_p(3,1)); q = q + 1; % case 2 % % % Calculation of the Max/Min Stroke: % % z = 8; % p = 1; % % while (p <= z) % % % Request m-File Leg_Calculation_Inverse_Kinematics % % Leg_Calculation_Inverse_Kinematics % % % Calculation of the travelling distances: % % r_11 = TCP {p} + ed_1 - leg_1 - bc_1; % r_21 = TCP {p} + ed_2 - leg_2 - bc_2; % r_31 = TCP {p} + ed_3 - leg_3 - bc_3; % % % Display of the calculation results: % % fprintf ('\n TCP: [%g; %g; %g] \n\n', TCP{p}(1), TCP{p}(2), TCP{p}(3)); % fprintf (' Stroke s1: %g mm \n', r_11(2,1)); % fprintf (' Storke s2: %g mm \n', r_21(2,1)); % fprintf (' Stroke s3: %g mm \n', r_31(2,1)); % % fprintf ('\n leg_1: [%g; %g; %g] \n', leg_1(1), leg_2(2), leg_2(3)); % fprintf (' leg_2: [%g; %g; %g] \n', leg_2(1), leg_2(2), leg_2(3)); % fprintf (' leg_3: [%g; %g; %g] \n', leg_3(1), leg_3(2), leg_3(3)); % % % Request m-File Test_Strokes_Inverse_Kinematics % % Test_Strokes_Inverse_Kinematics % % % % Calculation of the maximum stroke for s_1: % % if (r_11(2,1) > max_s1) % % max_s1 = r_11(2,1); % % elseif (r_11(2,1) < min_s1) % % min_s1 = r_11(2,1); % end % % % Calculation of the maximum stroke for s_2: % % if (r_21(2,1) > max_s2) % % max_s2 = r_21(2,1); % % elseif (r_21(2,1) < min_s2) % % min_s2 = r_21(2,1); % end % % % Calculation of the maximum stroke for s_3: % % if (r_31(2,1) > max_s3) % % max_s3 = r_31(2,1); % % elseif (r_31(2,1) < min_s3) % % min_s3 = r_31(2,1); % end % % p = p + 1; % % end % % stroke_s1 = max_s1 - min_s1; % stroke_s2 = max_s2 - min_s2; % stroke_s3 = max_s3 - min_s3; % % fprintf ('\n Maximum stroke s1: %g mm\n', stroke_s1); % fprintf ('\n Maximum stroke s2: %g mm\n', stroke_s2); % fprintf ('\n Maximum stroke s3: %g mm\n', stroke_s3); case 3 break end end