From 1098338bf0095c9c2fded4e893fb4d714b8fe8e3 Mon Sep 17 00:00:00 2001 From: ROS Station Date: Wed, 31 Jul 2019 10:49:21 +0200 Subject: [PATCH] cleaned up MATLAB scrips --- MATLAB/SmarGonFKIK/FKIK_symbolic.m | 172 ++++++++++ MATLAB/SmarGonFKIK/FKsmargon.m | 253 ++++++++++++++ MATLAB/SmarGonFKIK/IKeng.m | 146 ++++++++ MATLAB/SmarGonFKIK/IKsmargon.m | 320 ++++++++++++++++++ MATLAB/SmarGonFKIK/Test_calc.m | 73 ++++ MATLAB/SmarGonFKIK/Test_create_CATIA_laws.m | 35 ++ MATLAB/SmarGonFKIK/calcFK.m | 97 ++++++ MATLAB/SmarGonFKIK/calcIK.m | 155 +++++++++ MATLAB/SmarGonFKIK/calcIK_CATIA.m | 32 ++ MATLAB/SmarGonFKIK/newton2.m | 39 +++ MATLAB/SmarGonFKIK/newton_poly.m | 91 +++++ MATLAB/SmarGonFKIK/smargon_laws.txt | 19 ++ MATLAB/SmarGonFKIK/test.m | 104 ++++++ MATLAB/SmarGonFKIK/test2.m | 93 +++++ MATLAB/{ => motion}/Motion Calculation.pdf | Bin MATLAB/{ => motion}/Test.m | 0 MATLAB/{ => motion}/fastest_track_algorithm.m | 0 MATLAB/{ => motion}/fastest_track_function.m | 0 MATLAB/{ => motion}/min_jerk_xy_algorithm.m | 0 MATLAB/{ => motion}/pathmotion_inc.m | 0 MATLAB/{ => motion}/pt.m | 0 MATLAB/{ => motion}/pt4.m | 0 MATLAB/{ => motion}/pt4Test.m | 0 MATLAB/{ => motion}/ptTest.m | 0 MATLAB/{ => motion}/pvt.m | 0 MATLAB/{ => motion}/pvtTest.m | 0 .../{ => motion}/pvt_polynomial_algorithm.m | 0 .../pvt_polynomial_algorithm_stretch.m | 0 MATLAB/{ => motion}/tmp.m | 0 MATLAB/{ => motion}/trajectory.py | 0 30 files changed, 1629 insertions(+) create mode 100644 MATLAB/SmarGonFKIK/FKIK_symbolic.m create mode 100644 MATLAB/SmarGonFKIK/FKsmargon.m create mode 100644 MATLAB/SmarGonFKIK/IKeng.m create mode 100644 MATLAB/SmarGonFKIK/IKsmargon.m create mode 100644 MATLAB/SmarGonFKIK/Test_calc.m create mode 100644 MATLAB/SmarGonFKIK/Test_create_CATIA_laws.m create mode 100644 MATLAB/SmarGonFKIK/calcFK.m create mode 100644 MATLAB/SmarGonFKIK/calcIK.m create mode 100644 MATLAB/SmarGonFKIK/calcIK_CATIA.m create mode 100644 MATLAB/SmarGonFKIK/newton2.m create mode 100644 MATLAB/SmarGonFKIK/newton_poly.m create mode 100644 MATLAB/SmarGonFKIK/smargon_laws.txt create mode 100644 MATLAB/SmarGonFKIK/test.m create mode 100644 MATLAB/SmarGonFKIK/test2.m rename MATLAB/{ => motion}/Motion Calculation.pdf (100%) rename MATLAB/{ => motion}/Test.m (100%) rename MATLAB/{ => motion}/fastest_track_algorithm.m (100%) rename MATLAB/{ => motion}/fastest_track_function.m (100%) rename MATLAB/{ => motion}/min_jerk_xy_algorithm.m (100%) rename MATLAB/{ => motion}/pathmotion_inc.m (100%) rename MATLAB/{ => motion}/pt.m (100%) rename MATLAB/{ => motion}/pt4.m (100%) rename MATLAB/{ => motion}/pt4Test.m (100%) rename MATLAB/{ => motion}/ptTest.m (100%) rename MATLAB/{ => motion}/pvt.m (100%) rename MATLAB/{ => motion}/pvtTest.m (100%) rename MATLAB/{ => motion}/pvt_polynomial_algorithm.m (100%) rename MATLAB/{ => motion}/pvt_polynomial_algorithm_stretch.m (100%) rename MATLAB/{ => motion}/tmp.m (100%) rename MATLAB/{ => motion}/trajectory.py (100%) diff --git a/MATLAB/SmarGonFKIK/FKIK_symbolic.m b/MATLAB/SmarGonFKIK/FKIK_symbolic.m new file mode 100644 index 0000000..0f69ec6 --- /dev/null +++ b/MATLAB/SmarGonFKIK/FKIK_symbolic.m @@ -0,0 +1,172 @@ +% FK/IK using homogenous matrices %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% This script demonstrates how homogenous matrices can be used to create +% both a forward kinematics (FK)and an inverse kinematics (IK) model of +% a serial kinematics manipulator, with 5 % motors (q1,q2,q3,q4,q5) +% and 4 user coordinates (X,Y,Z,theta). +% +% The script uses a chain of Homogenous Transformation Matrices to create +% the FK model. And the Moore-Penrose Pseudo-Inverse of the Jacobian +% to iteratively solve the IK model (Newton-Raphson). +% +% 5.10.2017 Wayne Glettig + +clear all; +syms offsetX offsetY offsetZ ... + alpha beta gamma theta ... + q1 q2 q3 q4 q5 q6 ... + l01 ... + l11 l12 ... + l21 l22 l23... + l31 l32 l33 l34... + l41 l42... + l51 l52... + l61... + l71 l72 l73 l74... + SHX SHY SHZ; + + + +%% USER Coordinates +X = 1e-3; +Y = 1e-3; +Z = 0; +CHI = 0; +PHI = 0; +OMEGA = 0*pi/180; +%theta =0; + +%% Here the Transformation Matrix HBA is constructed from a translation +% matrix and 3 rotation matrices, using symbolic variables. +%Translation: +HXYZ = [1,0,0,offsetX;... + 0,1,0,offsetY;... + 0,0,1,offsetZ;... + 0,0,0,1]; +%Rot around X: +HRX = [1,0 ,0 ,0;... + 0,cos(alpha),-sin(alpha),0;... + 0,sin(alpha), cos(alpha),0;... + 0,0 ,0 ,1]; +%Rot around Y: +HRY = [ cos(beta),0,sin(beta),0;... + 0 ,1,0 ,0;... + -sin(beta),0,cos(beta),0;... + 0 ,0,0 ,1]; +%Rot around Z: +HRZ = [cos(gamma),-sin(gamma),0,0;... + sin(gamma), cos(gamma),0,0;... + 0 ,0 ,1,0;... + 0 ,0 ,0,1]; +%Create HBA (first rotation then translation) +HBA=HXYZ*HRZ*HRY*HRX; +%Create HAB (inverse) +HAB=inv(HBA); + +%% Functional lengths +% l01 = 42.5e-3; +% l11 = 25e-3 - (17e-3)/2; %half distance between sliders table midline +% l12 = l11; +% l21 = l11; +% l22 = l11; +% l23 = 13.5e-3; %Distance between q1 & q2 stage level +% l31 = 11.5e-3; %Distance from q3 table to middle of red part +% l32 = 68.5e-3 - (80e-3)/2; +% l33 = l31; +% l34 = l32; +% l41 = 76.5e-3; +% l42 = 25.5e-3; +% l51 = 10e-3; +% l52 = 2.5e-3; +% l61 = 64.422e-3; % Connecting rod length +% l71 = 5e-3; % Swing dimensions +% l72 = 17.67e-3; +% l73 = 5.2e-3; +% l74 = 1.53e-3; + + +%% Here the Kinematic Chain is created (q1,q2,q3,q4,q5 are motor coordinates) +%Essentially the HBA matrix above is filled with the offsetXYZ and angle +%values to assemble the chain. The chain consists here of the reference +%frames 0-1-2-3-4-5-6-7: +%Frame M: MATLAB Plot Frame (Z up) +%Frame 0: On Aerotech, before Omega +%Frame 1: On Blue Part +%Frame 2: On Green Part +%Frame 3: On Red Part (T-Part) +%Frame 4: On Orange Part +%Frame 5: On Olive Part +%Frame 6: On Purple Part (Connecting rod) +%Frame 7: On Yellow Part (Swing) +%Frame 8: On Pink part (Sample pin) + +H0M = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,0,0,pi/2,0,pi/2]); +H10 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l01,0,0,q6,0,0]); +H21 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,q1,0,0,0,0]); +H32 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l23,0,q2,0,0,0]); +H43 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l32+q3,l31,0,0,0,0]); +H53 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l34+q4,-l33,0,0,0,0]); +H35 = inv(H53); +H74 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l41,-l42,0,0,0,theta]); +H87 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l74,-l71,0,q5,0,0]); + + + + +%% The FK model (forward kinematics) can be defined as +% [X;Y;Z;OMEGA;CHI;PHI] = f(q1,q2,q3,q4,q5,q6,SHX,SHY,SHZ) + +% % H74 has theta in it, which must be calculated. +% % Distance between P560in5 and P770in5 +P560in5 = [l51;-l52;0;1]; +P770in7 = [-l72;-l71-l73;0;1]; +% we need H35 = inv(H53) +H35=inv(H53); +P770in5 = H35*H43*H74*P770in7; +eqn = sqrt((P770in5(1)-P560in5(1))^2+(P770in5(2)-P560in5(2))^2+(P770in5(3)-P560in5(3))^2) - l61; +%th = vpasolve(eqn,theta,0); %Look more into this. +% H74 = vpa(subs(H74,theta,th)); + +% Output point (P800) +P800in8 = [SHX;SHY;SHZ;1]; +H80 = H10*H21*H32*H43*H74*H87; +P800in0 = H80*P800in8; +X=P800in0(1); +Y=P800in0(2); +Z=P800in0(3); +OMEGA = q6*180/pi; +CHI = theta*180/pi; +PHI = q5*180/pi; + +f(1,1) = X; +f(2,1) = Y; +f(3,1) = Z; +f(4,1) = OMEGA; +f(5,1) = CHI; +f(6,1) = PHI; + +%%Get jacobian + +J= jacobian(f,[q1,q2,q3,theta,q5,q6]); % SO FAR ANALYTICAL. FROM HERE ON NUMERICAL. +Jpinv = pinv(J); %Moore-Penrose Pseudoinverse + +%% Calculate q4 based on q3 and theta +% % H35 has q4 in it, which must be calculated. +% % Distance between P560in5 and P770in5 +P560in5 = [l51;-l52;0;1]; +P770in7 = [-l72;-l71-l73;0;1]; +% we need H35 = inv(H53) +H35=inv(H53); +P770in5 = H35*H43*H74*P770in7; +eqn = sqrt((P770in5(1)-P560in5(1))^2+(P770in5(2)-P560in5(2))^2+(P770in5(3)-P560in5(3))^2) - l61; +eqn_d_theta = diff(eqn, theta) +eqn_d_q4 = diff(eqn, q4) + diff --git a/MATLAB/SmarGonFKIK/FKsmargon.m b/MATLAB/SmarGonFKIK/FKsmargon.m new file mode 100644 index 0000000..ef86326 --- /dev/null +++ b/MATLAB/SmarGonFKIK/FKsmargon.m @@ -0,0 +1,253 @@ +% FK/IK using homogenous matrices %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% This script demonstrates how homogenous matrices can be used to create +% both a forward kinematics (FK)and an inverse kinematics (IK) model of +% a serial kinematics manipulator, with 5 % motors (q1,q2,q3,q4,q5) +% and 4 user coordinates (X,Y,Z,theta). +% +% The script uses a chain of Homogenous Transformation Matrices to create +% the FK model. And the Moore-Penrose Pseudo-Inverse of the Jacobian +% to iteratively solve the IK model (Newton-Raphson). +% +% 5.10.2017 Wayne Glettig + +clear all; +syms offsetX offsetY offsetZ ... + alpha beta gamma theta; +% q1 q2 q3 q4 q5 q6; + + +%% MOTOR Coordinates +q1 = 0.0190; +q2 = -0.0273; +q3 = 0.0255; +q4 = 0.0107; +q5 = 0; +q6 = 0; +%theta =0; +SHX=0; +SHY=0; +SHZ=27.35e-3; + + +%% Here the Transformation Matrix HBA is constructed from a translation +% matrix and 3 rotation matrices, using symbolic variables. +%Translation: +HXYZ = [1,0,0,offsetX;... + 0,1,0,offsetY;... + 0,0,1,offsetZ;... + 0,0,0,1]; +%Rot around X: +HRX = [1,0 ,0 ,0;... + 0,cos(alpha),-sin(alpha),0;... + 0,sin(alpha), cos(alpha),0;... +%Rot around Y: + 0,0 ,0 ,1]; +HRY = [ cos(beta),0,sin(beta),0;... + 0 ,1,0 ,0;... + -sin(beta),0,cos(beta),0;... + 0 ,0,0 ,1]; +%Rot around Z: +HRZ = [cos(gamma),-sin(gamma),0,0;... + sin(gamma), cos(gamma),0,0;... + 0 ,0 ,1,0;... + 0 ,0 ,0,1]; +%Create HBA (first rotation then translation) +HBA=HXYZ*HRZ*HRY*HRX; +%Create HAB (inverse) +HAB=inv(HBA); + +%% Functional lengths +l01 = 42.5e-3; +l11 = 25e-3 - (17e-3)/2; %half distance between sliders table midline +l12 = l11; +l21 = l11; +l22 = l11; +l23 = 13.5e-3; %Distance between q1 & q2 stage level +l31 = 11.5e-3; %Distance from q3 table to middle of red part +l32 = 68.5e-3 - (80e-3)/2; +l33 = l31; +l34 = l32; +l41 = 76.5e-3; +l42 = 25.5e-3; +l51 = 10e-3; +l52 = 2.5e-3; +l61 = 64.422e-3; % Connecting rod length +l71 = 5e-3; % Swing dimensions +l72 = 17.67e-3; +l73 = 5.2e-3; +l74 = 1.53e-3; + + +%% Here the Kinematic Chain is created (q1,q2,q3,q4,q5 are motor coordinates) +%Essentially the HBA matrix above is filled with the offsetXYZ and angle +%values to assemble the chain. The chain consists here of the reference +%frames 0-1-2-3-4-5-6-7: +%Frame M: MATLAB Plot Frame (Z up) +%Frame 0: On Aerotech, before Omega +%Frame 1: On Blue Part +%Frame 2: On Green Part +%Frame 3: On Red Part (T-Part) +%Frame 4: On Orange Part +%Frame 5: On Olive Part +%Frame 6: On Purple Part (Connecting rod) +%Frame 7: On Yellow Part (Swing) +%Frame 8: On Pink part (Sample pin) + +H0M = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,0,0,pi/2,0,pi/2]); +H10 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l01,0,0,q6,0,0]); +H21 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,q1,0,0,0,0]); +H32 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l23,0,q2,0,0,0]); +H43 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l32+q3,l31,0,0,0,0]); +H53 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l34+q4,-l33,0,0,0,0]); +H74 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l41,-l42,0,0,0,theta]); +H87 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l74,-l71,0,q5,0,0]); + + + + +%% The FK model (forward kinematics) can be defined as +% [X;Y;Z;OMEGA;CHI;PHI] = f(q1,q2,q3,q4,q5,q6,SHX,SHY,SHZ) + +% H74 has theta in it, which must be calculated. +% Distance between P560in5 and P770in5 +P560in5 = [l51;-l52;0;1]; +P770in7 = [-l72;-l71-l73;0;1]; +% we need H35 = inv(H53) +H35=inv(H53); +P770in5 = H35*H43*H74*P770in7; + +eqn = sqrt((P770in5(1)-P560in5(1))^2+(P770in5(2)-P560in5(2))^2+(P770in5(3)-P560in5(3))^2) - l61; +th = vpasolve(eqn,theta,0); %Look more into this. +H74 = vpa(subs(H74,theta,th)); + +% Output point (P800) +P800in8 = [SHX;SHY;SHZ;1]; +H80 = H10*H21*H32*H43*H74*H87; +P800in0 = H80*P800in8; +X=P800in0(1) +Y=P800in0(2) +Z=P800in0(3) +OMEGA = q6*180/pi +CHI = vpa(th*180/pi) +PHI = q5*180/pi + + + +%% Plot the results (construct sample position using motors) +Oin1 = [0;0;0;1]; +e1in1 = [1;0;0;1]; +e2in1 = [0;1;0;1]; +e3in1 = [0;0;1;1]; + +%clf; +hold on; +grid on; +axis vis3d; +%axis([-10 10 -10 10 -10 10]); +title('SmarGon Direct Model (q1,q2,q3,q4,q5)'); +xlabel('Beamline Z-Axis'); +ylabel('Beamline X-Axis'); +zlabel('Beamline Y-Axis'); + +% Build Base (Black part) +P000in0 = [0;0;0;1]; +P100in0 = [l01;0;0;1]; +P000inM = H0M*P000in0; +P100inM = H0M*P100in0; +plot3([P000inM(1),P100inM(1)],[P000inM(2),P100inM(2)],[P000inM(3),P100inM(3)],'k-'); + +% Build Blue part, RF1 +P100in1 = [0;0;0;1]; +P110in1 = [0;0;l11;1]; +P111in1 = [0;0;-l11;1]; +P100inM = H0M*H10*P100in1; +P110inM = H0M*H10*P110in1; +P111inM = H0M*H10*P111in1; +plot3([P110inM(1),P100inM(1),P111inM(1)],[P110inM(2),P100inM(2),P111inM(2)],[P110inM(3),P100inM(3),P111inM(3)],'b+-'); + + +% Build Green part, RF2 +P210in2 = [0;0;l21;1]; +P211in2 = [0;0;-l21;1]; +P213in2 = [0;0;0;1]; +P220in2 = [l23;l31;0;1]; +P221in2 = [l23;-l31;0;1]; +P223in2 = [l23;0;0;1]; +P210inM = H0M*H10*H21*P210in2; +P211inM = H0M*H10*H21*P211in2; +P213inM = H0M*H10*H21*P213in2; +P220inM = H0M*H10*H21*P220in2; +P221inM = H0M*H10*H21*P221in2; +P223inM = H0M*H10*H21*P223in2; +plot3([P210inM(1),P213inM(1),P211inM(1)],[P210inM(2),P213inM(2),P211inM(2)],[P210inM(3),P213inM(3),P211inM(3)],'g+-'); +plot3([P213inM(1),P223inM(1)],[P213inM(2),P223inM(2)],[P213inM(3),P223inM(3)],'g+-'); +plot3([P220inM(1),P223inM(1),P221inM(1)],[P220inM(2),P223inM(2),P221inM(2)],[P220inM(3),P223inM(3),P221inM(3)],'g+-'); + +% Build Red part, RF3 +P320in3 = [0;l31;0;1]; +P321in3 = [0;-l33;0;1]; +P323in3 = [0;0;0;1]; +P330in3 = [l32;l31;0;1]; +P340in3 = [l34;-l33;0;1]; +P320inM = H0M*H10*H21*H32*P320in3; +P321inM = H0M*H10*H21*H32*P321in3; +P323inM = H0M*H10*H21*H32*P323in3; +P330inM = H0M*H10*H21*H32*P330in3; +P340inM = H0M*H10*H21*H32*P340in3; +plot3([P320inM(1),P323inM(1),P321inM(1)],[P320inM(2),P323inM(2),P321inM(2)],[P320inM(3),P323inM(3),P321inM(3)],'r+-'); +plot3([P320inM(1),P330inM(1)],[P320inM(2),P330inM(2)],[P320inM(3),P330inM(3)],'r+-'); +plot3([P321inM(1),P340inM(1)],[P321inM(2),P340inM(2)],[P321inM(3),P340inM(3)],'r+-'); + +% Build Orange part, RF4 +P430in4 = [0;0;0;1]; +P440in4 = [l41;0;0;1]; +P450in4 = [l41;-l42;0;1]; +P430inM = H0M*H10*H21*H32*H43*P430in4; +P440inM = H0M*H10*H21*H32*H43*P440in4; +P450inM = H0M*H10*H21*H32*H43*P450in4; +plot3([P430inM(1),P440inM(1),P450inM(1)],[P430inM(2),P440inM(2),P450inM(2)],[P430inM(3),P440inM(3),P450inM(3)],'+-', 'color',[250/255,140/255,0]); + +% Build Olive part, RF5 +P540in5 = [0;0;0;1]; +P550in5 = [l51;0;0;1]; +P560in5 = [l51;-l52;0;1]; +P540inM = H0M*H10*H21*H32*H53*P540in5; +P550inM = H0M*H10*H21*H32*H53*P550in5; +P560inM = H0M*H10*H21*H32*H53*P560in5; +plot3([P540inM(1),P550inM(1),P560inM(1)],[P540inM(2),P550inM(2),P560inM(2)],[P540inM(3),P550inM(3),P560inM(3)],'+-', 'color',[122/255,108/255,0]); + + +% Build Yellow part, RF7 +P750in7 = [0;0;0;1]; +P751in7 = [0;-l71;0;1]; +P770in7 = [-l72;-l71-l73;0;1]; +P771in7 = [-l72;-l71;0;1]; +P780in7 = [l74;-l71;0;1]; + +P750inM = H0M*H10*H21*H32*H43*H74*P750in7; +P751inM = H0M*H10*H21*H32*H43*H74*P751in7; +P770inM = H0M*H10*H21*H32*H43*H74*P770in7; +P771inM = H0M*H10*H21*H32*H43*H74*P771in7; +P780inM = H0M*H10*H21*H32*H43*H74*P780in7; + +%% plot it +plot3([P750inM(1),P751inM(1),P771inM(1),P770inM(1)],[P750inM(2),P751inM(2),P771inM(2),P770inM(2)],[P750inM(3),P751inM(3),P771inM(3),P770inM(3)],'+-', 'color',[255/255,255/255,128/255]); +plot3([P751inM(1),P780inM(1)],[P751inM(2),P780inM(2)],[P751inM(3),P780inM(3)],'+-', 'color',[255/255,255/255,128/255]); + + +%Distance between P560inM and P770inM +d= sqrt((P770inM(1)-P560inM(1))^2+(P770inM(2)-P560inM(2))^2+(P770inM(3)-P560inM(3))^2) +l61 + + + + diff --git a/MATLAB/SmarGonFKIK/IKeng.m b/MATLAB/SmarGonFKIK/IKeng.m new file mode 100644 index 0000000..92a2e9b --- /dev/null +++ b/MATLAB/SmarGonFKIK/IKeng.m @@ -0,0 +1,146 @@ +% SmarGon IK model, bare minimum %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% This script is a simplification of the more complete IKsmargon Script +% It tries to do away with all fancy functions, and should work on +% rather basic infrastructure. +% +% 22.2.2018 Wayne Glettig + +clear all; +%% USER Coordinates +X = 1e-3; +Y = 1e-3; +Z = 0; +CHI = 0; +PHI = 0; +OMEGA = 0*pi/180; +%theta =0; +SHX=0; +SHY=0; +SHZ=0; + + +%% Functional lengths +l01 = 42.5e-3; +l11 = 25e-3 - (17e-3)/2; %half distance between sliders table midline +l12 = l11; +l21 = l11; +l22 = l11; +l23 = 13.5e-3; %Distance between q1 & q2 stage level +l31 = 11.5e-3; %Distance from q3 table to middle of red part +l32 = 68.5e-3 - (80e-3)/2; +l33 = l31; +l34 = l32; +l41 = 76.5e-3; +l42 = 25.5e-3; +l51 = 10e-3; +l52 = 2.5e-3; +l61 = 64.422e-3; % Connecting rod length +l71 = 5e-3; % Swing dimensions +l72 = 17.67e-3; +l73 = 5.2e-3; +l74 = 1.53e-3; + + +%% the FK model is: + +% X= l01 + l23 + l32 + l41 + q3 + SHX*cos(theta) + l74*cos(theta) + l71*sin(theta) - SHY*cos(q5)*sin(theta) + SHZ*sin(q5)*sin(theta); +% Y= l31*cos(q6) - SHZ*(cos(q5)*sin(q6) + cos(q6)*cos(theta)*sin(q5)) - SHY*(sin(q5)*sin(q6) - cos(q5)*cos(q6)*cos(theta)) - l42*cos(q6) + q1*cos(q6) - q2*sin(q6) + SHX*cos(q6)*sin(theta) - l71*cos(q6)*cos(theta) + l74*cos(q6)*sin(theta); +% Z= SHY*(cos(q6)*sin(q5) + cos(q5)*cos(theta)*sin(q6)) + SHZ*(cos(q5)*cos(q6) - cos(theta)*sin(q5)*sin(q6)) + q2*cos(q6) + l31*sin(q6) - l42*sin(q6) + q1*sin(q6) + SHX*sin(q6)*sin(theta) - l71*cos(theta)*sin(q6) + l74*sin(q6)*sin(theta); +% OMEGA= (180*q6)/pi; +% CHI= (180*theta)/pi; +% PHI= (180*q5)/pi; + + +%% The IK model using the Pseudo-inverse of the Jacobian matrix +% to converge a solution towards a solution +% q are motor coords, x are user coords) + +xt=[X;Y;Z;OMEGA;CHI;PHI]; %SET TARGET USER COORDS (X,Y,Z,OMEGA,CHI,PHI) + +qc=[1e-3;1e-3;0;0;0;0]; %motor start values (q1,q2,q3,theta,q5,q6) +loopcond = 1; +loopcounter=0; +maxloops=30; +while loopcond + %get current x values based on q + xc(1,1)= l01 + l23 + l32 + l41 + q3 + SHX*cos(qc(4)) + l74*cos(qc(4)) + l71*sin(qc(4)) - SHY*cos(qc(5))*sin(qc(4)) + SHZ*sin(qc(5))*sin(qc(4)); + xc(2,1)= l31*cos(qc(6)) - SHZ*(cos(qc(5))*sin(qc(6)) + cos(qc(6))*cos(qc(4))*sin(qc(5))) - SHY*(sin(qc(5))*sin(qc(6)) - cos(qc(5))*cos(qc(6))*cos(qc(4))) - l42*cos(qc(6)) + q1*cos(qc(6)) - q2*sin(qc(6)) + SHX*cos(qc(6))*sin(qc(4)) - l71*cos(qc(6))*cos(qc(4)) + l74*cos(qc(6))*sin(qc(4)); + xc(3,1)= SHY*(cos(qc(6))*sin(qc(5)) + cos(qc(5))*cos(theta)*sin(qc(6))) + SHZ*(cos(qc(5))*cos(qc(6)) - cos(theta)*sin(qc(5))*sin(qc(6))) + q2*cos(qc(6)) + l31*sin(qc(6)) - l42*sin(qc(6)) + q1*sin(qc(6)) + SHX*sin(qc(6))*sin(qc(4)) - l71*cos(qc(4))*sin(qc(6)) + l74*sin(qc(6))*sin(qc(4)); + xc(4,1)= (180*qc(6))/pi; + xc(5,1)= (180*qc(4))/pi; + xc(6,1)= (180*qc(5))/pi; + + deltax=xt-xc; %get x error (target - current) + if abs(deltax)<1e-9 %if abs error small enough, get out of loop + loopcond=0; + disp('solution found') + end + %Calculate the Jacobian: + J= [ 0, 0, 1, l71*cos(theta) - SHX*sin(theta) - l74*sin(theta) - SHY*cos(qc(5))*cos(theta) + SHZ*cos(theta)*sin(qc(5)), SHZ*cos(qc(5))*sin(theta) + SHY*sin(qc(5))*sin(theta), 0;... + cos(qc(6)), -sin(qc(6)), 0, SHX*cos(qc(6))*cos(theta) + l74*cos(qc(6))*cos(theta) + l71*cos(qc(6))*sin(theta) - SHY*cos(qc(5))*cos(qc(6))*sin(theta) + SHZ*cos(qc(6))*sin(qc(5))*sin(theta), SHZ*(sin(qc(5))*sin(qc(6)) - cos(qc(5))*cos(qc(6))*cos(theta)) - SHY*(cos(qc(5))*sin(qc(6)) + cos(qc(6))*cos(theta)*sin(qc(5))), l42*sin(qc(6)) - SHZ*(cos(qc(5))*cos(qc(6)) - cos(theta)*sin(qc(5))*sin(qc(6))) - q2*cos(qc(6)) - l31*sin(qc(6)) - SHY*(cos(qc(6))*sin(qc(5)) + cos(qc(5))*cos(theta)*sin(qc(6))) - q1*sin(qc(6)) - SHX*sin(qc(6))*sin(theta) + l71*cos(theta)*sin(qc(6)) - l74*sin(qc(6))*sin(theta);... + sin(qc(6)), cos(qc(6)), 0, SHX*cos(theta)*sin(qc(6)) + l74*cos(theta)*sin(qc(6)) + l71*sin(qc(6))*sin(theta) - SHY*cos(qc(5))*sin(qc(6))*sin(theta) + SHZ*sin(qc(5))*sin(qc(6))*sin(theta), SHY*(cos(qc(5))*cos(qc(6)) - cos(theta)*sin(qc(5))*sin(qc(6))) - SHZ*(cos(qc(6))*sin(qc(5)) + cos(qc(5))*cos(theta)*sin(qc(6))), l31*cos(qc(6)) - SHZ*(cos(qc(5))*sin(qc(6)) + cos(qc(6))*cos(theta)*sin(qc(5))) - SHY*(sin(qc(5))*sin(qc(6)) - cos(qc(5))*cos(qc(6))*cos(theta)) - l42*cos(qc(6)) + q1*cos(qc(6)) - q2*sin(qc(6)) + SHX*cos(qc(6))*sin(theta) - l71*cos(qc(6))*cos(theta) + l74*cos(qc(6))*sin(theta);... + 0, 0, 0, 0, 0, 180/pi;... + 0, 0, 0, 180/pi, 0, 0;... + 0, 0, 0, 0, 180/pi, 0]; + Jinvc = pinv(J); + deltaq=Jinvc*deltax; %By multiplying the x error with Jinv, a q correction can be deduced + qc = qc+deltaq;%update current motor values + + loopcounter=loopcounter+1; + if loopcounter > maxloops + loopcond = 0; + disp('30 iterations reached') + end +end +q = qc %output q as the motor coordinates + +%% The IK model using the Pseudo-inverse of the Jacobian matrix +% to converge a solution towards a solution + +% Jacobian of FK f function derived over motor coordinates + + + +J= jacobian(f,[q1,q2,q3,theta,q5,q6]); % SO FAR ANALYTICAL. FROM HERE ON NUMERICAL. + +Jinv = pinv(J); + +tic +% Algorithm for IK (q are motor coords, x are user coords) +xt=[0.1687391293583430989690637491127;-0.015209423663512846293490651681675;0.002;0;47.990543436670047241427846041931;0.2865]; %SET TARGET USER COORDS +q0=[1e-3;1e-3;0;0;0;0]; %motor start values +qc = q0; %set current q values for loop +loopcond = 1; +loopcounter=0; +while loopcond + xc = vpa(subs(f, [q1,q2,q3,theta,q5,q6], qc')); %get current x values based on q + deltax=xt-xc; %get x error (target - current) + if abs(deltax)<1e-9 %if abs error small enough, get out of loop + loopcond=0; + disp('error small enough') + end + Jinvc=vpa(subs(Jinv, [q1,q2,q3,theta,q5,q6], qc')); %inv Jacobian with current q + deltaq=Jinvc*deltax; %By multiplying the x error with Jinv, a q correction can be deduced + qc = qc+deltaq;%update current motor values + loopcounter=loopcounter+1; + if loopcounter > 30 + loopcond = 0; + disp('30 iterations reached') + end +end +q = qc %output q as the motor coordinates +toc + +%% Calculate q4 based on q3 and theta +% % H35 has q4 in it, which must be calculated. +% % Distance between P560in5 and P770in5 +P560in5 = [l51;-l52;0;1]; +P770in7 = [-l72;-l71-l73;0;1]; +% we need H35 = inv(H53) +H35=inv(H53); +P770in5 = H35*H43*H74*P770in7; +P770in5= subs(P770in5, [q1,q2,q3,theta,q5,q6], qc') +eqn = sqrt((P770in5(1)-P560in5(1))^2+(P770in5(2)-P560in5(2))^2+(P770in5(3)-P560in5(3))^2) - l61; +q4_calc = vpasolve(eqn,q4,0); %Look more into this. +% H74 = vpa(subs(H74,theta,th)); diff --git a/MATLAB/SmarGonFKIK/IKsmargon.m b/MATLAB/SmarGonFKIK/IKsmargon.m new file mode 100644 index 0000000..6db8dea --- /dev/null +++ b/MATLAB/SmarGonFKIK/IKsmargon.m @@ -0,0 +1,320 @@ +% FK/IK using homogenous matrices %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% This script demonstrates how homogenous matrices can be used to create +% both a forward kinematics (FK)and an inverse kinematics (IK) model of +% a serial kinematics manipulator, with 5 % motors (q1,q2,q3,q4,q5) +% and 4 user coordinates (X,Y,Z,theta). +% +% The script uses a chain of Homogenous Transformation Matrices to create +% the FK model. And the Moore-Penrose Pseudo-Inverse of the Jacobian +% to iteratively solve the IK model (Newton-Raphson). +% +% 5.10.2017 Wayne Glettig + +clear all; +syms offsetX offsetY offsetZ ... + alpha beta gamma theta ... + q1 q2 q3 q4 q5 q6; + + + + +%% USER Coordinates +X = 1e-3; +Y = 1e-3; +Z = 0; +CHI = 0; +PHI = 0; +OMEGA = 0*pi/180; +%theta =0; +SHX=0; +SHY=0; +SHZ=0; + + +%% Functional lengths +l01 = 42.5e-3; +l11 = 25e-3 - (17e-3)/2; %half distance between sliders table midline +l12 = l11; +l21 = l11; +l22 = l11; +l23 = 13.5e-3; %Distance between q1 & q2 stage level +l31 = 11.5e-3; %Distance from q3 table to middle of red part +l32 = 68.5e-3 - (80e-3)/2; +l33 = l31; +l34 = l32; +l41 = 76.5e-3; +l42 = 25.5e-3; +l51 = 10e-3; +l52 = 2.5e-3; +l61 = 64.422e-3; % Connecting rod length +l71 = 5e-3; % Swing dimensions +l72 = 17.67e-3; +l73 = 5.2e-3; +l74 = 1.53e-3; + + +%% Here the Transformation Matrix HBA is constructed from a translation +% matrix and 3 rotation matrices, using symbolic variables. +%Translation: +HXYZ = [1,0,0,offsetX;... + 0,1,0,offsetY;... + 0,0,1,offsetZ;... + 0,0,0,1]; +%Rot around X: +HRX = [1,0 ,0 ,0;... + 0,cos(alpha),-sin(alpha),0;... + 0,sin(alpha), cos(alpha),0;... + 0,0 ,0 ,1]; +%Rot around Y: +HRY = [ cos(beta),0,sin(beta),0;... + 0 ,1,0 ,0;... + -sin(beta),0,cos(beta),0;... + 0 ,0,0 ,1]; +%Rot around Z: +HRZ = [cos(gamma),-sin(gamma),0,0;... + sin(gamma), cos(gamma),0,0;... + 0 ,0 ,1,0;... + 0 ,0 ,0,1]; +%Create HBA (first rotation then translation) +HBA=HXYZ*HRZ*HRY*HRX; +%Create HAB (inverse) +HAB=inv(HBA); + + +%% Here the Kinematic Chain is created (q1,q2,q3,q4,q5 are motor coordinates) +%Essentially the HBA matrix above is filled with the offsetXYZ and angle +%values to assemble the chain. The chain consists here of the reference +%frames 0-1-2-3-4-5-6-7: +%Frame M: MATLAB Plot Frame (Z up) +%Frame 0: On Aerotech, before Omega +%Frame 1: On Blue Part +%Frame 2: On Green Part +%Frame 3: On Red Part (T-Part) +%Frame 4: On Orange Part +%Frame 5: On Olive Part +%Frame 6: On Purple Part (Connecting rod) +%Frame 7: On Yellow Part (Swing) +%Frame 8: On Pink part (Sample pin) + +H0M = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,0,0,pi/2,0,pi/2]); +H10 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l01,0,0,q6,0,0]); +H21 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,q1,0,0,0,0]); +H32 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l23,0,q2,0,0,0]); +H43 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l32+q3,l31,0,0,0,0]); +H53 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l34+q4,-l33,0,0,0,0]); +H35 = inv(H53); +H74 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l41,-l42,0,0,0,theta]); +H87 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l74,-l71,0,q5,0,0]); + + + + +%% The FK model (forward kinematics) can be defined as +% [X;Y;Z;OMEGA;CHI;PHI] = f(q1,q2,q3,q4,q5,q6,SHX,SHY,SHZ) + +% % H74 has theta in it, which must be calculated. +% % Distance between P560in5 and P770in5 +% P560in5 = [l51;-l52;0;1]; +% P770in7 = [-l72;-l71-l73;0;1]; +% % we need H35 = inv(H53) +% H35=inv(H53); +% P770in5 = H35*H43*H74*P770in7; +% +% eqn = sqrt((P770in5(1)-P560in5(1))^2+(P770in5(2)-P560in5(2))^2+(P770in5(3)-P560in5(3))^2) - l61; +% th = vpasolve(eqn,theta,0); %Look more into this. +% H74 = vpa(subs(H74,theta,th)); + +% Output point (P800) +P800in8 = [SHX;SHY;SHZ;1]; +H80 = H10*H21*H32*H43*H74*H87; +P800in0 = H80*P800in8; +X=P800in0(1) +Y=P800in0(2) +Z=P800in0(3) +OMEGA = q6*180/pi +CHI = theta*180/pi +PHI = q5*180/pi + +f(1,1) = X; +f(2,1) = Y; +f(3,1) = Z; +f(4,1) = OMEGA; +f(5,1) = CHI; +f(6,1) = PHI; + + +%% The IK model is more difficult to find. Different IK solution methods +% exist, this is one of the basic ones, using the Pseudo-inverse of the +% Jacobian matrix to converge a solution towards a solution + +% Jacobian of FK f function derived over motor coordinates +J= jacobian(f,[q1,q2,q3,theta,q5,q6]); % SO FAR ANALYTICAL. FROM HERE ON NUMERICAL. + +Jinv = pinv(J); + +tic +% Algorithm for IK (q are motor coords, x are user coords) +xt=[0.1687391293583430989690637491127;-0.015209423663512846293490651681675;0.002;0;47.990543436670047241427846041931;0.2865]; %SET TARGET USER COORDS +q0=[1e-3;1e-3;0;0;0;0]; %motor start values +qc = q0; %set current q values for loop +loopcond = 1; +loopcounter=0; +while loopcond + xc = vpa(subs(f, [q1,q2,q3,theta,q5,q6], qc')); %get current x values based on q + deltax=xt-xc; %get x error (target - current) + if abs(deltax)<1e-9 %if abs error small enough, get out of loop + loopcond=0; + disp('error small enough') + end + Jinvc=vpa(subs(Jinv, [q1,q2,q3,theta,q5,q6], qc')); %inv Jacobian with current q + deltaq=Jinvc*deltax; %By multiplying the x error with Jinv, a q correction can be deduced + qc = qc+deltaq;%update current motor values + loopcounter=loopcounter+1; + if loopcounter > 30 + loopcond = 0; + disp('30 iterations reached') + end +end +q = qc %output q as the motor coordinates +toc + +%% Calculate q4 based on q3 and theta +% % H35 has q4 in it, which must be calculated. +% % Distance between P560in5 and P770in5 +P560in5 = [l51;-l52;0;1]; +P770in7 = [-l72;-l71-l73;0;1]; +% we need H35 = inv(H53) +H35=inv(H53); +P770in5 = H35*H43*H74*P770in7; +eqn = sqrt((P770in5(1)-P560in5(1))^2+(P770in5(2)-P560in5(2))^2+(P770in5(3)-P560in5(3))^2) - l61; + +q4_calc = vpasolve(eqn,q4,0); %Look more into this. +% H74 = vpa(subs(H74,theta,th)); + +%% Plot the results (construct sample position using motors) +Oin1 = [0;0;0;1]; +e1in1 = [1;0;0;1]; +e2in1 = [0;1;0;1]; +e3in1 = [0;0;1;1]; + +%clf; +hold on; +grid on; +axis vis3d; +%axis([-10 10 -10 10 -10 10]); +title('SmarGon Direct Model (q1,q2,q3,q4,q5)'); +xlabel('Beamline Z-Axis'); +ylabel('Beamline X-Axis'); +zlabel('Beamline Y-Axis'); + +% Build Base (Black part) +P000in0 = [0;0;0;1]; +P100in0 = [l01;0;0;1]; +P000inM = H0M*P000in0; +P100inM = H0M*P100in0; +plot3([P000inM(1),P100inM(1)],[P000inM(2),P100inM(2)],[P000inM(3),P100inM(3)],'k-'); + +% Build Blue part, RF1 +P100in1 = [0;0;0;1]; +P110in1 = [0;0;l11;1]; +P111in1 = [0;0;-l11;1]; +P100inM = H0M*H10*P100in1; +P110inM = H0M*H10*P110in1; +P111inM = H0M*H10*P111in1; +plot3([P110inM(1),P100inM(1),P111inM(1)],[P110inM(2),P100inM(2),P111inM(2)],[P110inM(3),P100inM(3),P111inM(3)],'b+-'); + + +% Build Green part, RF2 +P210in2 = [0;0;l21;1]; +P211in2 = [0;0;-l21;1]; +P213in2 = [0;0;0;1]; +P220in2 = [l23;l31;0;1]; +P221in2 = [l23;-l31;0;1]; +P223in2 = [l23;0;0;1]; +P210inM = H0M*H10*H21*P210in2; +P211inM = H0M*H10*H21*P211in2; +P213inM = H0M*H10*H21*P213in2; +P220inM = H0M*H10*H21*P220in2; +P221inM = H0M*H10*H21*P221in2; +P223inM = H0M*H10*H21*P223in2; +plot3([P210inM(1),P213inM(1),P211inM(1)],[P210inM(2),P213inM(2),P211inM(2)],[P210inM(3),P213inM(3),P211inM(3)],'g+-'); +plot3([P213inM(1),P223inM(1)],[P213inM(2),P223inM(2)],[P213inM(3),P223inM(3)],'g+-'); +plot3([P220inM(1),P223inM(1),P221inM(1)],[P220inM(2),P223inM(2),P221inM(2)],[P220inM(3),P223inM(3),P221inM(3)],'g+-'); + +% Build Red part, RF3 +P320in3 = [0;l31;0;1]; +P321in3 = [0;-l33;0;1]; +P323in3 = [0;0;0;1]; +P330in3 = [l32;l31;0;1]; +P340in3 = [l34;-l33;0;1]; +P320inM = H0M*H10*H21*H32*P320in3; +P321inM = H0M*H10*H21*H32*P321in3; +P323inM = H0M*H10*H21*H32*P323in3; +P330inM = H0M*H10*H21*H32*P330in3; +P340inM = H0M*H10*H21*H32*P340in3; +plot3([P320inM(1),P323inM(1),P321inM(1)],[P320inM(2),P323inM(2),P321inM(2)],[P320inM(3),P323inM(3),P321inM(3)],'r+-'); +plot3([P320inM(1),P330inM(1)],[P320inM(2),P330inM(2)],[P320inM(3),P330inM(3)],'r+-'); +plot3([P321inM(1),P340inM(1)],[P321inM(2),P340inM(2)],[P321inM(3),P340inM(3)],'r+-'); + +% Build Orange part, RF4 +P430in4 = [0;0;0;1]; +P440in4 = [l41;0;0;1]; +P450in4 = [l41;-l42;0;1]; +P430inM = H0M*H10*H21*H32*H43*P430in4; +P440inM = H0M*H10*H21*H32*H43*P440in4; +P450inM = H0M*H10*H21*H32*H43*P450in4; +plot3([P430inM(1),P440inM(1),P450inM(1)],[P430inM(2),P440inM(2),P450inM(2)],[P430inM(3),P440inM(3),P450inM(3)],'+-', 'color',[250/255,140/255,0]); + +% Build Olive part, RF5 +P540in5 = [0;0;0;1]; +P550in5 = [l51;0;0;1]; +P560in5 = [l51;-l52;0;1]; +P540inM = H0M*H10*H21*H32*H53*P540in5; +P550inM = H0M*H10*H21*H32*H53*P550in5; +P560inM = H0M*H10*H21*H32*H53*P560in5; +plot3([P540inM(1),P550inM(1),P560inM(1)],[P540inM(2),P550inM(2),P560inM(2)],[P540inM(3),P550inM(3),P560inM(3)],'+-', 'color',[122/255,108/255,0]); + + +% Build Yellow part, RF7 +P750in7 = [0;0;0;1]; +P751in7 = [0;-l71;0;1]; +P770in7 = [-l72;-l71-l73;0;1]; +P771in7 = [-l72;-l71;0;1]; +P780in7 = [l74;-l71;0;1]; + +%% Calculate theta, based on q3 and q4: +P770inM = H0M*H10*H21*H32*H43*H74*P770in7; + +%Distance between P560inM and P770inM +eqn = sqrt((P770inM(1)-P560inM(1))^2+(P770inM(2)-P560inM(2))^2+(P770inM(3)-P560inM(3))^2) - l61 +th = vpasolve(eqn,theta,0) + +H74 = vpa(subs(H74,theta,th)); + +P750inM = H0M*H10*H21*H32*H43*H74*P750in7; +P751inM = H0M*H10*H21*H32*H43*H74*P751in7; +P770inM = H0M*H10*H21*H32*H43*H74*P770in7; +P771inM = H0M*H10*H21*H32*H43*H74*P771in7; +P780inM = H0M*H10*H21*H32*H43*H74*P780in7; + +%% plot it +plot3([P750inM(1),P751inM(1),P771inM(1),P770inM(1)],[P750inM(2),P751inM(2),P771inM(2),P770inM(2)],[P750inM(3),P751inM(3),P771inM(3),P770inM(3)],'+-', 'color',[255/255,255/255,128/255]); +plot3([P751inM(1),P780inM(1)],[P751inM(2),P780inM(2)],[P751inM(3),P780inM(3)],'+-', 'color',[255/255,255/255,128/255]); + + +%Distance between P560inM and P770inM +d= sqrt((P770inM(1)-P560inM(1))^2+(P770inM(2)-P560inM(2))^2+(P770inM(3)-P560inM(3))^2) +l61 + + + + diff --git a/MATLAB/SmarGonFKIK/Test_calc.m b/MATLAB/SmarGonFKIK/Test_calc.m new file mode 100644 index 0000000..4643cef --- /dev/null +++ b/MATLAB/SmarGonFKIK/Test_calc.m @@ -0,0 +1,73 @@ +%% This Script tests the calcIK and calkFK models. +clear all; + +%% MOTOR Coordinates +q1 = 1e-3; +q2 = 2e-3; +q3 = 2e-3; +q4 = 2e-3; +q5 = 5e-3; +q6 = 0*pi/180; +[X,Y,Z,OMEGA,CHI,PHI] = calcFK(q1,q2,q3,q4,q5,q6); +[q1,q2,q3,q4,q5,q6] = calcIK(X,Y,Z,OMEGA,CHI,PHI); + +%% USER Coordinates +X = 188e-3; +Y = 0; +Z = 0; +OMEGA = 0*pi/180; +CHI = 0; +PHI = 0; +[q1,q2,q3,q4,q5,q6] = calcIK(X,Y,Z,OMEGA,CHI,PHI) +[X,Y,Z,OMEGA,CHI,PHI] = calcFK(q1,q2,q3,q4,q5,q6) + + +%% Write CATIA Law file +fid = fopen('smargon_laws.txt','wt'); +fprintf(fid, '// Law for Befehl.1 bis Befehl.6\n'); +fprintf(fid, '---------------------------------\n'); +fprintf(fid, '*COLUMNS = *TIME, Befehl.1, Befehl.2, Befehl.3, Befehl.4, Befehl.5, Befehl.6\n'); +fprintf(fid, '*INTERPOLATION = polyline,spline,polyline,polyline,polyline,polyline\n'); +fprintf(fid, '*UNIT = Deg,m,m,m,m,Deg\n'); +fprintf(fid, '*YSCALE = 1,1,1,1,1,1\n'); +t=2; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,0,0,0,0,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=4; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,10e-3,0,0,0,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=6; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,10e-3,10e-3,0,0,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=8; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,-10e-3,10e-3,0,0,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=10; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,-10e-3,-10e-3,0,0,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=12; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,10e-3,0,0,0,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=14; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,0,0,0,0,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=15; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,0,0,0,20,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=16; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,0,0,0,40,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=17; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,0,0,0,50,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=18; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,0,0,0,60,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=19; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,0,0,0,70,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +t=20; +[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(188e-3,0,0,0,90,0); +fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); + +fclose(fid); \ No newline at end of file diff --git a/MATLAB/SmarGonFKIK/Test_create_CATIA_laws.m b/MATLAB/SmarGonFKIK/Test_create_CATIA_laws.m new file mode 100644 index 0000000..74dc952 --- /dev/null +++ b/MATLAB/SmarGonFKIK/Test_create_CATIA_laws.m @@ -0,0 +1,35 @@ +%% This script creates a CATIA Law file with the current points. + +% Here you can specify the points to be reached: +% points = [time, X, Y, Z, OMEGA, CHI, PHI; ...] +points = [ 2, 188e-3, 0, 0, 0, 0, 0;... + 4, 188e-3, 10e-3, 0, 0, 0, 0;... + 6, 188e-3, 10e-3, 10e-3, 0, 0, 0;... + 8, 188e-3,-10e-3, 10e-3, 0, 0, 0;... + 10, 188e-3,-10e-3,-10e-3, 0, 0, 0;... + 12, 188e-3, 10e-3, 0, 10e-3, 0, 0;... + 14, 188e-3, 0, 0, 0, 20, 0;... + 15, 188e-3, 0, 0, 0, 40, 0;... + 16, 188e-3, 0, 0, 0, 50, 0;... + 17, 188e-3, 0, 0, 0, 60, 0;... + 18, 188e-3, 0, 0, 0, 70, 0;... + 19, 188e-3, 0, 0, 0, 80, 0;... + 10, 188e-3, 0, 0, 0, 90, 0]; + +number_of_points = size(points,1) + +%% Write CATIA Law file +fid = fopen('smargon_laws.txt','wt'); +fprintf(fid, '// Law for Befehl.1 bis Befehl.6\n'); +fprintf(fid, '---------------------------------\n'); +fprintf(fid, '*COLUMNS = *TIME, Befehl.1, Befehl.2, Befehl.3, Befehl.4, Befehl.5, Befehl.6\n'); +fprintf(fid, '*INTERPOLATION = polyline,spline,polyline,polyline,polyline,polyline\n'); +fprintf(fid, '*UNIT = Deg,m,m,m,m,Deg\n'); +fprintf(fid, '*YSCALE = 1,1,1,1,1,1\n'); +for i = 1:number_of_points + t=points(i,1); + [b1,b2,b3,b4,b5,b6] = calcIK_CATIA(points(i,2),points(i,3),points(i,4),points(i,5),points(i,6),points(i,7)); + fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6); +end; + +fclose(fid); \ No newline at end of file diff --git a/MATLAB/SmarGonFKIK/calcFK.m b/MATLAB/SmarGonFKIK/calcFK.m new file mode 100644 index 0000000..f2c91f2 --- /dev/null +++ b/MATLAB/SmarGonFKIK/calcFK.m @@ -0,0 +1,97 @@ +function [X,Y,Z,OMEGA,CHI,PHI] = calcFK(q1,q2,q3,q4,q5,q6) +% SmarGon IK model, bare minimum %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% This script is a simplification of the more complete FKsmargon script +% It tries to do away with all fancy functions, and should work on +% rather basic infrastructure. +% +% 22.2.2018 Wayne Glettig + + +%% MOTOR Coordinates +% q1 = 1e-3; +% q2 = 2e-3; +% q3 = 2e-3; +% q4 = 2e-3; +% q5 = 5e-3; +% q6 = 0*pi/180; + +SHX=27.35e-3; +SHY=0; +SHZ=0; + +%% Functional lengths +l01 = 42.5e-3; +l11 = 25e-3 - (17e-3)/2; %half distance between sliders table midline +l12 = l11; +l21 = l11; +l22 = l11; +l23 = 13.5e-3; %Distance between q1 & q2 stage level +l31 = 11.5e-3; %Distance from q3 table to middle of red part +l32 = 68.5e-3 - (80e-3)/2; +l33 = l31; +l34 = l32; +l41 = 76.5e-3; +l42 = 25.5e-3; +l51 = 10e-3; +l52 = 2.5e-3; +l61 = 64.422e-3; % Connecting rod length +l71 = 5e-3; % Swing dimensions +l72 = 17.67e-3; +l73 = 5.2e-3; +l74 = 1.53e-3; + + +%% Calculate theta (based on q3 & q4) +% calculate eqn (using symbolic math in IKsmargon) +%eqn = ((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2) - l61 +% get the derivative of eqn (in symbolic math diff(eqn, theta)) +%eqn_diff =(2*(cos(theta)*(l71 + l73) + l72*sin(theta))*(l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta)) + 2*(sin(theta)*(l71 + l73) - l72*cos(theta))*(l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta)))/(2*((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2)) + +% Use the newton method: +% Starting value theta_0: +theta = 0; +% Set loop conditions +loopcond = 1; +loopcounter =0; +maxloops = 30; +while (loopcond == 1) + %Newton Formula, using f, and f_diff, + f = ((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2) - l61; + f_diff = (2*(cos(theta)*(l71 + l73) + l72*sin(theta))*(l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta)) + 2*(sin(theta)*(l71 + l73) - l72*cos(theta))*(l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta)))/(2*((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2)); + theta = theta - (f/f_diff); + % Increase loop counter, and stop looping if maxloop is reached + loopcounter = loopcounter +1; + if (loopcounter> maxloops) + loopcond = 0; + %ERROR no solution found!!!! + end + % Calculate residual error, and if sufficiently small stop looping + if (abs(f/f_diff)<1e-9) + loopcond = 0; + end +end + +theta; + +% Visualize it: +fplot(@(theta) ((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2) - l61, [-pi pi]); +grid on; +hold on; +title ('eqn = f(theta), eqn=0 -> solutions '); +xlabel('theta angle'); +ylabel('eqn'); +plot (theta, ((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2) - l61, 'o'); + + +%% the FK model is: + +X= l01 + l23 + l32 + l41 + q3 + SHX*cos(theta) + l74*cos(theta) + l71*sin(theta) - SHY*cos(q5)*sin(theta) + SHZ*sin(q5)*sin(theta); +Y= l31*cos(q6) - SHZ*(cos(q5)*sin(q6) + cos(q6)*cos(theta)*sin(q5)) - SHY*(sin(q5)*sin(q6) - cos(q5)*cos(q6)*cos(theta)) - l42*cos(q6) + q1*cos(q6) - q2*sin(q6) + SHX*cos(q6)*sin(theta) - l71*cos(q6)*cos(theta) + l74*cos(q6)*sin(theta); +Z= SHY*(cos(q6)*sin(q5) + cos(q5)*cos(theta)*sin(q6)) + SHZ*(cos(q5)*cos(q6) - cos(theta)*sin(q5)*sin(q6)) + q2*cos(q6) + l31*sin(q6) - l42*sin(q6) + q1*sin(q6) + SHX*sin(q6)*sin(theta) - l71*cos(theta)*sin(q6) + l74*sin(q6)*sin(theta); +OMEGA= (180*q6)/pi; +CHI= (180*theta)/pi; +PHI= (180*q5)/pi; + +end + diff --git a/MATLAB/SmarGonFKIK/calcIK.m b/MATLAB/SmarGonFKIK/calcIK.m new file mode 100644 index 0000000..ca79224 --- /dev/null +++ b/MATLAB/SmarGonFKIK/calcIK.m @@ -0,0 +1,155 @@ +function [q1,q2,q3,q4,q5,q6] = calcIK(X,Y,Z,OMEGA,CHI,PHI) +% SmarGon IK model, bare minimum %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% This script is a simplification of the more complete IKsmargon Script +% It tries to do away with all fancy functions, and should work on +% rather basic infrastructure. +% +% 22.2.2018 Wayne Glettig + + +%% USER Coordinates +% X = 0.1676; +% Y = -0.0154; +% Z = 0.0020; +% OMEGA = 0*pi/180; +% CHI = 45.3100; +% PHI = 0.2865; + +SHX=27.35e-3; +SHY=0; +SHZ=0; + + +%% Functional lengths +l01 = 42.5e-3; +l11 = 25e-3 - (17e-3)/2; %half distance between sliders table midline +l12 = l11; +l21 = l11; +l22 = l11; +l23 = 13.5e-3; %Distance between q1 & q2 stage level +l31 = 11.5e-3; %Distance from q3 table to middle of red part +l32 = 68.5e-3 - (80e-3)/2; +l33 = l31; +l34 = l32; +l41 = 76.5e-3; +l42 = 25.5e-3; +l51 = 10e-3; +l52 = 2.5e-3; +l61 = 64.422e-3; % Connecting rod length +l71 = 5e-3; % Swing dimensions +l72 = 17.67e-3; +l73 = 5.2e-3; +l74 = 1.53e-3; + + +%% the FK model is: + +% X= l01 + l23 + l32 + l41 + q3 + SHX*cos(theta) + l74*cos(theta) + l71*sin(theta) - SHY*cos(q5)*sin(theta) + SHZ*sin(q5)*sin(theta); +% Y= l31*cos(q6) - SHZ*(cos(q5)*sin(q6) + cos(q6)*cos(theta)*sin(q5)) - SHY*(sin(q5)*sin(q6) - cos(q5)*cos(q6)*cos(theta)) - l42*cos(q6) + q1*cos(q6) - q2*sin(q6) + SHX*cos(q6)*sin(theta) - l71*cos(q6)*cos(theta) + l74*cos(q6)*sin(theta); +% Z= SHY*(cos(q6)*sin(q5) + cos(q5)*cos(theta)*sin(q6)) + SHZ*(cos(q5)*cos(q6) - cos(theta)*sin(q5)*sin(q6)) + q2*cos(q6) + l31*sin(q6) - l42*sin(q6) + q1*sin(q6) + SHX*sin(q6)*sin(theta) - l71*cos(theta)*sin(q6) + l74*sin(q6)*sin(theta); +% OMEGA= (180*q6)/pi; +% CHI= (180*theta)/pi; +% PHI= (180*q5)/pi; + + +%% The IK model using the Pseudo-inverse of the Jacobian matrix +% to converge a solution towards a solution +% q are motor coords, x are user coords) + +xt=[X;Y;Z;OMEGA;CHI;PHI]; %SET TARGET USER COORDS (X,Y,Z,OMEGA,CHI,PHI) + +qc=[1e-3;1e-3;0;0;0;0]; %motor start values (q1,q2,q3,theta,q5,q6) +loopcond = 1; +loopcounter=0; +maxloops=30; +while loopcond + %get current x values based on q + xc(1,1)= l01 + l23 + l32 + l41 + qc(3) + SHX*cos(qc(4)) + l74*cos(qc(4)) + l71*sin(qc(4)) - SHY*cos(qc(5))*sin(qc(4)) + SHZ*sin(qc(5))*sin(qc(4)); + xc(2,1)= l31*cos(qc(6)) - SHZ*(cos(qc(5))*sin(qc(6)) + cos(qc(6))*cos(qc(4))*sin(qc(5))) - SHY*(sin(qc(5))*sin(qc(6)) - cos(qc(5))*cos(qc(6))*cos(qc(4))) - l42*cos(qc(6)) + qc(1)*cos(qc(6)) - qc(2)*sin(qc(6)) + SHX*cos(qc(6))*sin(qc(4)) - l71*cos(qc(6))*cos(qc(4)) + l74*cos(qc(6))*sin(qc(4)); + xc(3,1)= SHY*(cos(qc(6))*sin(qc(5)) + cos(qc(5))*cos(qc(4))*sin(qc(6))) + SHZ*(cos(qc(5))*cos(qc(6)) - cos(qc(4))*sin(qc(5))*sin(qc(6))) + qc(2)*cos(qc(6)) + l31*sin(qc(6)) - l42*sin(qc(6)) + qc(1)*sin(qc(6)) + SHX*sin(qc(6))*sin(qc(4)) - l71*cos(qc(4))*sin(qc(6)) + l74*sin(qc(6))*sin(qc(4)); + xc(4,1)= (180*qc(6))/pi; + xc(5,1)= (180*qc(4))/pi; + xc(6,1)= (180*qc(5))/pi; + + deltax=xt-xc; %get x error (target - current) + if abs(deltax)<1e-9 %if abs error small enough, get out of loop + loopcond=0; + disp('solution found') + end + %Calculate the Jacobian: + J= [ 0, 0, 1, l71*cos(qc(4)) - SHX*sin(qc(4)) - l74*sin(qc(4)) - SHY*cos(qc(5))*cos(qc(4)) + SHZ*cos(qc(4))*sin(qc(5)), SHZ*cos(qc(5))*sin(qc(4)) + SHY*sin(qc(5))*sin(qc(4)), 0;... + cos(qc(6)), -sin(qc(6)), 0, SHX*cos(qc(6))*cos(qc(4)) + l74*cos(qc(6))*cos(qc(4)) + l71*cos(qc(6))*sin(qc(4)) - SHY*cos(qc(5))*cos(qc(6))*sin(qc(4)) + SHZ*cos(qc(6))*sin(qc(5))*sin(qc(4)), SHZ*(sin(qc(5))*sin(qc(6)) - cos(qc(5))*cos(qc(6))*cos(qc(4))) - SHY*(cos(qc(5))*sin(qc(6)) + cos(qc(6))*cos(qc(4))*sin(qc(5))), l42*sin(qc(6)) - SHZ*(cos(qc(5))*cos(qc(6)) - cos(qc(4))*sin(qc(5))*sin(qc(6))) - qc(2)*cos(qc(6)) - l31*sin(qc(6)) - SHY*(cos(qc(6))*sin(qc(5)) + cos(qc(5))*cos(qc(4))*sin(qc(6))) - qc(1)*sin(qc(6)) - SHX*sin(qc(6))*sin(qc(4)) + l71*cos(qc(4))*sin(qc(6)) - l74*sin(qc(6))*sin(qc(4));... + sin(qc(6)), cos(qc(6)), 0, SHX*cos(qc(4))*sin(qc(6)) + l74*cos(qc(4))*sin(qc(6)) + l71*sin(qc(6))*sin(qc(4)) - SHY*cos(qc(5))*sin(qc(6))*sin(qc(4)) + SHZ*sin(qc(5))*sin(qc(6))*sin(qc(4)), SHY*(cos(qc(5))*cos(qc(6)) - cos(qc(4))*sin(qc(5))*sin(qc(6))) - SHZ*(cos(qc(6))*sin(qc(5)) + cos(qc(5))*cos(qc(4))*sin(qc(6))), l31*cos(qc(6)) - SHZ*(cos(qc(5))*sin(qc(6)) + cos(qc(6))*cos(qc(4))*sin(qc(5))) - SHY*(sin(qc(5))*sin(qc(6)) - cos(qc(5))*cos(qc(6))*cos(qc(4))) - l42*cos(qc(6)) + qc(1)*cos(qc(6)) - qc(2)*sin(qc(6)) + SHX*cos(qc(6))*sin(qc(4)) - l71*cos(qc(6))*cos(qc(4)) + l74*cos(qc(6))*sin(qc(4));... + 0, 0, 0, 0, 0, 180/pi;... + 0, 0, 0, 180/pi, 0, 0;... + 0, 0, 0, 0, 180/pi, 0]; + Jinvc = pinv(J); + deltaq=Jinvc*deltax; %By multiplying the x error with Jinv, a q correction can be deduced + qc = qc+deltaq;%update current motor values + + loopcounter=loopcounter+1; + if loopcounter > maxloops + loopcond = 0; + disp('30 iterations reached') + end +end +%output the motor coordinates +q1 = qc(1); +q2 = qc(2); +q3 = qc(3); +theta = qc(4); +q5 = qc(5); +q6 = qc(6); + + + +%% Calculate q4 based on q3 and theta +% % H35 has q4 in it, which must be calculated. +% % Distance between P560in5 and P770in5 +P560in5 = [l51;-l52;0;1]; +P770in7 = [-l72;-l71-l73;0;1]; + +% calculate eqn (using symbolic math in FKIKsmargon) +%eqn = ((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2) - l61 +% This is the same equation as in the FK model, but the derivative is of q4 +% and not theta +% get the derivative of eqn (in symbolic math diff(eqn, theta)) +%eqn_d_q4 = -(2*l32 - 2*l34 + 2*l41 - 2*l51 + 2*q3 - 2*q4 + 2*sin(theta)*(l71 + l73) - 2*l72*cos(theta))/(2*((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2)) + +% Use the newton method: +% Starting value theta_0: +q4 = 0; +% Set loop conditions +loopcond = 1; +loopcounter =0; +maxloops = 30; +while (loopcond == 1) + %Newton Formula, using f, and f_diff, + f = ((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2) - l61; + f_diff = -(2*l32 - 2*l34 + 2*l41 - 2*l51 + 2*q3 - 2*q4 + 2*sin(theta)*(l71 + l73) - 2*l72*cos(theta))/(2*((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2)); + q4 = q4 - (f/f_diff); + % Increase loop counter, and stop looping if maxloop is reached + loopcounter = loopcounter +1; + if (loopcounter> maxloops) + loopcond = 0; + %ERROR no solution found!!!! + end + % Calculate residual error, and if sufficiently small stop looping + if (abs(f/f_diff)<1e-9) + loopcond = 0; + end +end + +q4; + +% Visualize it: +% fplot(@(q4) ((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2) - l61, [-50e-3 50e-3]); +% grid on; +% hold on; +% title ('eqn = f(q4), eqn=0 -> solutions '); +% xlabel('q4'); +% ylabel('eqn'); +% plot (q4, ((l32 - l34 + l41 - l51 + q3 - q4 + sin(theta)*(l71 + l73) - l72*cos(theta))^2 + (l31 + l33 - l42 + l52 - cos(theta)*(l71 + l73) - l72*sin(theta))^2)^(1/2) - l61, 'o'); + + diff --git a/MATLAB/SmarGonFKIK/calcIK_CATIA.m b/MATLAB/SmarGonFKIK/calcIK_CATIA.m new file mode 100644 index 0000000..a06737c --- /dev/null +++ b/MATLAB/SmarGonFKIK/calcIK_CATIA.m @@ -0,0 +1,32 @@ +function [b1,b2,b3,b4,b5,b6] = calcIK_CATIA(X,Y,Z,OMEGA,CHI,PHI) +%CALCIK_CATIA Summary of this function goes here +% Detailed explanation goes here + +[q1,q2,q3,q4,q5,q6] = calcIK(X,Y,Z,OMEGA,CHI,PHI) + +% calcIK variables +% q1 slider in Y direction +% q2 slider in Z direction +% q3 upper x slider +% q4 lower x slider +% q5 Phi Rotation +% q6 Omega Rotation + +% CATIA variables +% b1 Omega Rotation +% b2 slider in Y direction +% b3 slider in Z direction +% b4 upper x slider +% b5 lower x slider +% b6 Phi Rotation + + +b1 = q6 + 180; +b2 = -q1 -4.836e-3; +b3 = q2; +b4 = -q3 -6.837e-3; +b5 = -q4 -6.873e-3; +b6 = q5; + +end + diff --git a/MATLAB/SmarGonFKIK/newton2.m b/MATLAB/SmarGonFKIK/newton2.m new file mode 100644 index 0000000..a604db5 --- /dev/null +++ b/MATLAB/SmarGonFKIK/newton2.m @@ -0,0 +1,39 @@ +function [x , res , xvec , resvec] = newton( f, df, x0, maxiter, tol ) +% +% NEWTON Newton's Method +% Newton's method for finding better approximations to the +% zeroes of a real-valued function. +% +% Input: +% f - input funtion +% df - derived input function +% x0 - initial aproximation +% maxiter - maximum number of iterations allowed +% tol - the required tolerance +% +% Output: +% x - variable containing approximation to x(*) +% res- variable containing the residual |f(x)| evaluated at the approximate solution x +% xvec- vector containing {x(k)}, including initial guess and final approximation +% resvec- vector of the same dimension of xvec, containing {|f(xk)|}. +% +function x(k) = newton( f, df, x0, maxiter, tol ) +k(1:maxiter) +f = @x(k) +df = @(x(k))' +x(k)= x0-(f(x0)/df(x0)) +x(k+1) = x(k) - (f(x(k))/df(x(k))) +end +%continue iterating until stopping conditions met +%output depends on success of algorithm +while (iter < maxiter) + while (abs(f(n)-(f(n-1)) > tol) + x(1)= x0-(f(x0)/df(x0)) + elseif (iter > maxiter) + error('maximum amount of iterations exceeded') + else error('distance between last 2 points is less than stated tolerance') + end +end +fprintf('step(k) x(k) |f(xk)| |x(k+1)-x(k)|') +fprintf('------ ----------- --------- -------------\n') +fprintf('%d %d %.4f %.4f\n', k, x(k), mod(f(xk)), mod(x(k+1)?x(k)) ) \ No newline at end of file diff --git a/MATLAB/SmarGonFKIK/newton_poly.m b/MATLAB/SmarGonFKIK/newton_poly.m new file mode 100644 index 0000000..af98005 --- /dev/null +++ b/MATLAB/SmarGonFKIK/newton_poly.m @@ -0,0 +1,91 @@ +function [root,number_of_iteration] = newton_poly(vector,initial,tolerance,maxiteration) +% ----------------------------------------------------------- +% General usage: Newton's Method to find the roots of a polynomail +% +% Notes: +% the root function in the MATLAB library can find all the roots of +% a polynomial with arbitrary order. +% But this method, gives the one the roots based on the initial guess +% and it gives the number of iteration required to converge. +% +% Input: +% vector: The vector describing the polynomial +% initial: Initial guess +% tolerance: The desired error +% maxiteration: The maximum number of iteration +% +% Output: +% root: The detected root based on the initial guess +% number_of_iteration: number of iteraion to find the root +% +% +% Example: +% f(x)=(x^3)-6(X^2)+72(x)-27=0 +% therefore +% vector=[1 -6 -72 -27] +% initial=300; +% tolerance=10^-2; +% maxiteration=10^4; +% [root,number_of_iteration] = newton(vector,initial,tolerance,maxiteration) +% or +% [root,number_of_iteration] = newton([1 -6 -72 -27],300,10^-2,10^4) +% root= +% 12.1229 +% number_of_iteration= +% 13 +% This means that the detected root based on the initial +% guess (300) is 12.1229 and it converge after 7 iterations. +% +% Author: Farhad Sedaghati +% Contact: +% Written: 07/30/2015 +% +% ----------------------------------------------------------- +if nargin>4 + error('Too many input arguments'); +elseif nargin==3 + maxiteration=10^4; +elseif nargin==2 + maxiteration=10^4; + tolerance=10^-2; +elseif nargin<2 + error('Function needs more input arguments'); +end + +% switch the elements +vector(1:end)=vector(end:-1:1); +% Size of the vector +N=length(vector); +% initial guess +x=initial; +% dummy variable +sum=0; +for l=1:N + % evaluate the polynomial using intiail value + sum=sum+vector(l)*x.^(l-1); +end +number_of_iteration=0; +while abs(sum)>tolerance + number_of_iteration=number_of_iteration+1; + if number_of_iteration>maxiteration + error('Failed to converge, the maximum number of iterations is reached') + end + dif=0; + sum=0; + for k=2:N + % finding the deravitive at a specific point + dif=dif+(k-1)*vector(k)*x.^(k-2); + end + if dif==0 + error('The deravitve of the function is zero, peak another initial point and run again') + end + for l=1:N + % find the value of the polynomial at a specific point + sum=sum+vector(l)*x.^(l-1); + end + % substituting in the newton's formula + x=x-sum/dif; +end +root=x; + + diff --git a/MATLAB/SmarGonFKIK/smargon_laws.txt b/MATLAB/SmarGonFKIK/smargon_laws.txt new file mode 100644 index 0000000..b19d619 --- /dev/null +++ b/MATLAB/SmarGonFKIK/smargon_laws.txt @@ -0,0 +1,19 @@ +// Law for Befehl.1 bis Befehl.6 +--------------------------------- +*COLUMNS = *TIME, Befehl.1, Befehl.2, Befehl.3, Befehl.4, Befehl.5, Befehl.6 +*INTERPOLATION = polyline,spline,polyline,polyline,polyline,polyline +*UNIT = Deg,m,m,m,m,Deg +*YSCALE = 1,1,1,1,1,1 +2 180.000000 -0.023836 0.000000 -0.004957 0.009786 0.000000 +4 180.000000 -0.033836 0.000000 -0.004957 0.009786 0.000000 +6 180.000000 -0.033836 0.010000 -0.004957 0.009786 0.000000 +8 180.000000 -0.013836 0.010000 -0.004957 0.009786 0.000000 +10 180.000000 -0.013836 -0.010000 -0.004957 0.009786 0.000000 +12 180.000175 -0.033836 -0.000002 -0.004957 0.009786 0.000000 +14 180.000000 -0.013657 0.000000 -0.004989 0.004089 0.000000 +15 180.000000 -0.004103 -0.000000 -0.008500 -0.006553 0.000000 +16 180.000000 0.000073 0.000000 -0.011443 -0.013226 0.000000 +17 180.000000 0.003675 -0.000000 -0.015067 -0.020495 0.000000 +18 180.000000 0.006592 0.000000 -0.019261 -0.028130 0.000000 +19 180.000000 0.008737 -0.000000 -0.023898 -0.035908 0.000000 +10 180.000000 0.010044 -0.000000 -0.028837 -0.043622 0.000000 diff --git a/MATLAB/SmarGonFKIK/test.m b/MATLAB/SmarGonFKIK/test.m new file mode 100644 index 0000000..d4f6456 --- /dev/null +++ b/MATLAB/SmarGonFKIK/test.m @@ -0,0 +1,104 @@ +clear all; +syms offsetX offsetY offsetZ ... + alpha beta gamma... + q1 q2 q3 q4 q5 q6; + +%% Here the Transformation Matrix HBA is constructed from a translation +% matrix and 3 rotation matrices, using symbolic variables. +%Translation: +HXYZ = [1,0,0,offsetX;... + 0,1,0,offsetY;... + 0,0,1,offsetZ;... + 0,0,0,1]; +%Rot around X: +HRX = [1,0 ,0 ,0;... + 0,cos(alpha),-sin(alpha),0;... + 0,sin(alpha), cos(alpha),0;... + 0,0 ,0 ,1]; +%Rot around Y: +HRY = [ cos(beta),0,sin(beta),0;... + 0 ,1,0 ,0;... + -sin(beta),0,cos(beta),0;... + 0 ,0,0 ,1]; +%Rot around Z: +HRZ = [cos(gamma),-sin(gamma),0,0;... + sin(gamma), cos(gamma),0,0;... + 0 ,0 ,1,0;... + 0 ,0 ,0,1]; +%Create HBA (first rotation then translation) +HBA=HXYZ*HRZ*HRY*HRX; +%Create HAB (inverse) +HAB=inv(HBA); + + +l31 = 10e-3; +l32 = 20e-3; +l33 = 10e-3; +l34 = 20e-3; +l41 = 40e-3; +l42 = 30e-3; +l61 = 50e-3; +l71 = 10e-3; +l72 = 20e-3; +l73 = 10e-3; +l74 = 20e-3; + +syms theta tmp1 tmp2 tmp3 tmp4 tmp5 +%H73(via Part 4) = H73 (via Parts 5 & 6) +%H73(via Part 4): +H43 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [q3+l32,l31,0,0,0,0]); %Slider #3 +H74 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l41,-l42,0,0,0,theta]); %theta angle +H73a = H43*H74 + +%(only thing unknown is theta) + +%H73 (via Parts 5 & 6) +%H73 = H53*H65*H76 +H53 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [q4+l34,-l33,0, 0,0,0]); %Slider #4 + Ball joint +H65 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,0,l61,tmp1,tmp2,tmp3]); %long joint +H7a6 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [0,0,0,tmp4,tmp5,0]); %ball joint +H77a = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l72,-l71-l72,0,0,0,0]); +H73b = H53*H65*H7a6*H77a + %(unknowns tmp1 tmp2 tmp3 tmp4 tmp5) + +f= H73a(1:3,4)-H73b(1:3,4) +f1= H73a(1:3,1)-H73b(1:3,1) +f2= H73a(1:3,2)-H73b(1:3,2) +f3= H73a(1:3,3)-H73b(1:3,3) + +% Jacobian of FK f function derived over motor coordinates +J= jacobian(f,[theta,tmp1,tmp2,tmp3,tmp4,tmp5]); +J1= jacobian(f1,[theta,tmp1,tmp2,tmp3,tmp4,tmp5]); +J2= jacobian(f2,[theta,tmp1,tmp2,tmp3,tmp4,tmp5]); +J3= jacobian(f3,[theta,tmp1,tmp2,tmp3,tmp4,tmp5]); + +J= [J;J1;J2;J3] +Jinv = pinv(J); + v tic +% Algorithm for IK (q are motor coords, x are user coords) +xt=[2;2;2;30]; %SET TARGET US ER COORDS +q0=[0;0;0;0;0]; %motor start values +qc = q0; %set current q values for loop +loopcond = 1; +loopcounter=0; +while loopcond + xc = vpa(subs(f, [q1,q2,q3,q4,q5], qc')); %get current x values based on q + deltax=xt-xc; %get x error (target - current) + if (abs(deltax)<1e-9) | (loopcounter > 4)%if abs error small enough, get out of loop + loopcond=0; + end + Jinvc=vpa(subs(Jinv, [q1,q2,q3,q4,q5], qc')); %inv Jacobian with current q + deltaq=Jinvc*deltax; %By multiplying the x error with Jinv, a q correction can be deduced + qc = qc+deltaq;%update current motor values + loopcounter=loopcounter+1; +end +q = qc %output q as the motor coordinates +toc + + diff --git a/MATLAB/SmarGonFKIK/test2.m b/MATLAB/SmarGonFKIK/test2.m new file mode 100644 index 0000000..6fa4d45 --- /dev/null +++ b/MATLAB/SmarGonFKIK/test2.m @@ -0,0 +1,93 @@ +clear all; +syms offsetX offsetY offsetZ ... + alpha beta gamma; + +%% Here the Transformation Matrix HBA is constructed from a translation +% matrix and 3 rotation matrices, using symbolic variables. +%Translation: +HXYZ = [1,0,0,offsetX;... + 0,1,0,offsetY;... + 0,0,1,offsetZ;... + 0,0,0,1]; +%Rot around X: +HRX = [1,0 ,0 ,0;... + 0,cos(alpha),-sin(alpha),0;... + 0,sin(alpha), cos(alpha),0;... + 0,0 ,0 ,1]; +%Rot around Y: +HRY = [ cos(beta),0,sin(beta),0;... + 0 ,1,0 ,0;... + -sin(beta),0,cos(beta),0;... + 0 ,0,0 ,1]; +%Rot around Z: +HRZ = [cos(gamma),-sin(gamma),0,0;... + sin(gamma), cos(gamma),0,0;... + 0 ,0 ,1,0;... + 0 ,0 ,0,1]; +%Create HBA (first rotation then translation) +HBA=HXYZ*HRZ*HRY*HRX; +%Create HAB (inverse) +HAB=inv(HBA); + + +l31 = 10e-3; +l32 = 20e-3; +l33 = 10e-3; +l34 = 20e-3; +l41 = 40e-3; +l42 = 30e-3; +l51 = 50e-3; +l52 = 50e-3; +l61 = 50e-3; +l71 = 10e-3; +l72 = 20e-3; +l73 = 10e-3; +l74 = 20e-3; +q3=0; +q4=0; + + +syms x y z theta +H43 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l32+q3,l31,0,0,0,0]); +H53 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],... + [l34+q4,-l33,0,0,0,0]); +H35 = inv(H53) +H75 = H35*H43*H74 + +P770in7 = [-l72;-l71-l73;0;1] + + +f=H75*P770in7-[x;y;z;1] +f(4)=(x + l51)^2 + (y + l52)^2 + z^2 - l61^2 + + +% Jacobian of FK f function derived over the searched variables +J= jacobian(f,[x,y,z,theta]); + +Jinv = pinv(J); +tic +% Algorithm for IK (q are motor coords, x are user coords) +xt=[0;0;0;0]; %SET TARGET US ER COORDS +q0=[-l51+l61;-l52;0;0]; %motor start values +qc = q0; %set current q values for loop +loopcond = 1; +loopcounter=0; +while loopcond + xc = vpa(subs(f, [x,y,z,theta], qc')); %get current x values based on q + deltax=xt-xc %get x error (target - current) + if (abs(deltax)<1e-9) | (loopcounter > 4)%if abs error small enough, get out of loop + loopcond=0; + end + Jinvc=vpa(subs(Jinv, [x,y,z,theta], qc')); %inv Jacobian with current q + deltaq=Jinvc*deltax; %By multiplying the x error with Jinv, a q correction can be deduced + qc = qc+deltaq;%update current motor values + loopcounter=loopcounter+1; +end +q = qc %output q as the motor coordinates +toc + + + + + diff --git a/MATLAB/Motion Calculation.pdf b/MATLAB/motion/Motion Calculation.pdf similarity index 100% rename from MATLAB/Motion Calculation.pdf rename to MATLAB/motion/Motion Calculation.pdf diff --git a/MATLAB/Test.m b/MATLAB/motion/Test.m similarity index 100% rename from MATLAB/Test.m rename to MATLAB/motion/Test.m diff --git a/MATLAB/fastest_track_algorithm.m b/MATLAB/motion/fastest_track_algorithm.m similarity index 100% rename from MATLAB/fastest_track_algorithm.m rename to MATLAB/motion/fastest_track_algorithm.m diff --git a/MATLAB/fastest_track_function.m b/MATLAB/motion/fastest_track_function.m similarity index 100% rename from MATLAB/fastest_track_function.m rename to MATLAB/motion/fastest_track_function.m diff --git a/MATLAB/min_jerk_xy_algorithm.m b/MATLAB/motion/min_jerk_xy_algorithm.m similarity index 100% rename from MATLAB/min_jerk_xy_algorithm.m rename to MATLAB/motion/min_jerk_xy_algorithm.m diff --git a/MATLAB/pathmotion_inc.m b/MATLAB/motion/pathmotion_inc.m similarity index 100% rename from MATLAB/pathmotion_inc.m rename to MATLAB/motion/pathmotion_inc.m diff --git a/MATLAB/pt.m b/MATLAB/motion/pt.m similarity index 100% rename from MATLAB/pt.m rename to MATLAB/motion/pt.m diff --git a/MATLAB/pt4.m b/MATLAB/motion/pt4.m similarity index 100% rename from MATLAB/pt4.m rename to MATLAB/motion/pt4.m diff --git a/MATLAB/pt4Test.m b/MATLAB/motion/pt4Test.m similarity index 100% rename from MATLAB/pt4Test.m rename to MATLAB/motion/pt4Test.m diff --git a/MATLAB/ptTest.m b/MATLAB/motion/ptTest.m similarity index 100% rename from MATLAB/ptTest.m rename to MATLAB/motion/ptTest.m diff --git a/MATLAB/pvt.m b/MATLAB/motion/pvt.m similarity index 100% rename from MATLAB/pvt.m rename to MATLAB/motion/pvt.m diff --git a/MATLAB/pvtTest.m b/MATLAB/motion/pvtTest.m similarity index 100% rename from MATLAB/pvtTest.m rename to MATLAB/motion/pvtTest.m diff --git a/MATLAB/pvt_polynomial_algorithm.m b/MATLAB/motion/pvt_polynomial_algorithm.m similarity index 100% rename from MATLAB/pvt_polynomial_algorithm.m rename to MATLAB/motion/pvt_polynomial_algorithm.m diff --git a/MATLAB/pvt_polynomial_algorithm_stretch.m b/MATLAB/motion/pvt_polynomial_algorithm_stretch.m similarity index 100% rename from MATLAB/pvt_polynomial_algorithm_stretch.m rename to MATLAB/motion/pvt_polynomial_algorithm_stretch.m diff --git a/MATLAB/tmp.m b/MATLAB/motion/tmp.m similarity index 100% rename from MATLAB/tmp.m rename to MATLAB/motion/tmp.m diff --git a/MATLAB/trajectory.py b/MATLAB/motion/trajectory.py similarity index 100% rename from MATLAB/trajectory.py rename to MATLAB/motion/trajectory.py