wip
This commit is contained in:
142
python/FKIKfaststage.m
Normal file
142
python/FKIKfaststage.m
Normal file
@@ -0,0 +1,142 @@
|
||||
% 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...
|
||||
q1 q2 q3 q4 q5;
|
||||
|
||||
%% 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 0: MATLAB darstellungsframe (for plot3)
|
||||
%Frame 1: Beamline UCS
|
||||
%Frame 2: on Z Stage
|
||||
%Frame 3: on X Stage
|
||||
%Frame 4: on RX Stage
|
||||
%Frame 5: on Fast X Stage
|
||||
%Frame 6: on Fast Y Stage
|
||||
%Frame 7: Sample pos
|
||||
|
||||
H10 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],...
|
||||
[0,0,0,90/180*pi,0/180*pi,90/180*pi]);
|
||||
H21 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],...
|
||||
[0,0,q1,0,0,0]); %Z Stage
|
||||
H32 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],...
|
||||
[q2,0,0,0,0,0]); %X Stage
|
||||
H43 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],...
|
||||
[0,0,0,0,q3/180*pi,0]); %Rotation RY
|
||||
H54 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],...
|
||||
[q4,0,0,0,0,0]); %Fast Stage X
|
||||
H65 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],...
|
||||
[0,q5,0,0,0,0]); %Fast Stage Y
|
||||
H76 = subs(HBA,[offsetX,offsetY,offsetZ,alpha,beta,gamma],...
|
||||
[0,0,0,0,0,0]); %sample Offset
|
||||
H70 = H10*H21*H32*H43*H54*H65*H76;
|
||||
H07 = inv(H70);
|
||||
|
||||
%% The FK model (forward kinematics) can be defined as
|
||||
% [X;Y;Z;theta] = f(q1,q2,q3,q4,q5)
|
||||
H71 = H21*H32*H43*H54*H65*H76; %UCS: reference frame 1
|
||||
f= H71*[0;0;0;1]; %XYZ position of orgin of reference frame 7
|
||||
f(4)=q3; %theta angle directly connected to q3
|
||||
|
||||
%% 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
|
||||
% (Newton-Raphson)
|
||||
|
||||
% Jacobian of FK f function derived over motor coordinates
|
||||
J= jacobian(f,[q1,q2,q3,q4,q5]);
|
||||
Jinv = pinv(J);
|
||||
|
||||
tic
|
||||
% Algorithm for IK (q are motor coords, x are user coords)
|
||||
xt=[2;1;0;32]; %SET TARGET USER 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 %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
|
||||
|
||||
%% 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('Sample Position, constructed with (q1,q2,q3,q4,q5)');
|
||||
xlabel('Beamline Z-Axis');
|
||||
ylabel('Beamline X-Axis');
|
||||
zlabel('Beamline Y-Axis');
|
||||
|
||||
|
||||
for i= 0:1
|
||||
H60c = subs(H70, [q1 q2 q3, q4, q5], q');
|
||||
|
||||
Oin0 = H60c*Oin1;
|
||||
e1in0 = H60c*e1in1;
|
||||
e2in0 = H60c*e2in1;
|
||||
e3in0 = H60c*e3in1;
|
||||
|
||||
plot3([Oin0(1),e1in0(1)],[Oin0(2),e1in0(2)],[Oin0(3),e1in0(3)],'r-');
|
||||
plot3([Oin0(1),e2in0(1)],[Oin0(2),e2in0(2)],[Oin0(3),e2in0(3)],'g-');
|
||||
plot3([Oin0(1),e3in0(1)],[Oin0(2),e3in0(2)],[Oin0(3),e3in0(3)],'b-');
|
||||
plot3([-10,0],[0,0],[0,0],'r-')
|
||||
end
|
||||
Reference in New Issue
Block a user