cleaned up MATLAB scrips
This commit is contained in:
ROS Station
172
MATLAB/SmarGonFKIK/FKIK_symbolic.m
Normal file
172
MATLAB/SmarGonFKIK/FKIK_symbolic.m
Normal file
@@ -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)
|
||||
|
||||
253
MATLAB/SmarGonFKIK/FKsmargon.m
Normal file
253
MATLAB/SmarGonFKIK/FKsmargon.m
Normal file
@@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
146
MATLAB/SmarGonFKIK/IKeng.m
Normal file
146
MATLAB/SmarGonFKIK/IKeng.m
Normal file
@@ -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));
|
||||
320
MATLAB/SmarGonFKIK/IKsmargon.m
Normal file
320
MATLAB/SmarGonFKIK/IKsmargon.m
Normal file
@@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
73
MATLAB/SmarGonFKIK/Test_calc.m
Normal file
73
MATLAB/SmarGonFKIK/Test_calc.m
Normal file
@@ -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);
|
||||
35
MATLAB/SmarGonFKIK/Test_create_CATIA_laws.m
Normal file
35
MATLAB/SmarGonFKIK/Test_create_CATIA_laws.m
Normal file
@@ -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);
|
||||
97
MATLAB/SmarGonFKIK/calcFK.m
Normal file
97
MATLAB/SmarGonFKIK/calcFK.m
Normal file
@@ -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
|
||||
|
||||
155
MATLAB/SmarGonFKIK/calcIK.m
Normal file
155
MATLAB/SmarGonFKIK/calcIK.m
Normal file
@@ -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');
|
||||
|
||||
|
||||
32
MATLAB/SmarGonFKIK/calcIK_CATIA.m
Normal file
32
MATLAB/SmarGonFKIK/calcIK_CATIA.m
Normal file
@@ -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
|
||||
|
||||
39
MATLAB/SmarGonFKIK/newton2.m
Normal file
39
MATLAB/SmarGonFKIK/newton2.m
Normal file
@@ -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)) )
|
||||
91
MATLAB/SmarGonFKIK/newton_poly.m
Normal file
91
MATLAB/SmarGonFKIK/newton_poly.m
Normal file
@@ -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: <farhad_seda@yahoo.com>
|
||||
% 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;
|
||||
|
||||
|
||||
19
MATLAB/SmarGonFKIK/smargon_laws.txt
Normal file
19
MATLAB/SmarGonFKIK/smargon_laws.txt
Normal file
@@ -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
|
||||
104
MATLAB/SmarGonFKIK/test.m
Normal file
104
MATLAB/SmarGonFKIK/test.m
Normal file
@@ -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
|
||||
|
||||
|
||||
93
MATLAB/SmarGonFKIK/test2.m
Normal file
93
MATLAB/SmarGonFKIK/test2.m
Normal file
@@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user