gistfile1.txt

XYZ_ref = [ simOut.EndEffector_AbsPos{1}.Values.Data,simOut.EndEffector_AbsPos{2}.Values.Data,simOut.EndEffector_AbsPos{3}.Values.Data ]';

Nsim = size(Gantry_out_D.Data,1);
XYZ = zeros(3,Nsim);
for k=1:Nsim
XYZ(:,k) = ForwardKinematicsFun(Gantry_out_D.Data(k,2),...
                     Axis1_Out_D.Data(k,2)/180*pi,...
                     Axis2_Out_D.Data(k,2)/180*pi,...
                     Axis3_Out_D.Data(k,2)/180*pi,...
                     Axis4_Out_D.Data(k,2)/180*pi,...
                     Axis5_Out_D.Data(k,2)/180*pi,...
                     Axis6_Out_D.Data(k,2)/180*pi);
end

figure
hold on
plot3(simOut.EndEffector_AbsPos{1}.Values.Data,simOut.EndEffector_AbsPos{2}.Values.Data,simOut.EndEffector_AbsPos{3}.Values.Data)
scatter3(simOut.EndEffector_AbsPos{1}.Values.Data(1),simOut.EndEffector_AbsPos{2}.Values.Data(1),simOut.EndEffector_AbsPos{3}.Values.Data(1))
plot3(XYZ(1,:),XYZ(2,:),XYZ(3,:))
scatter3(XYZ(1,1),XYZ(2,1),XYZ(3,1))

xlabel('x')
ylabel('y')
zlabel('z')
axis square

%%
%  R = SerialLink(dh, options) is a robot object with kinematics defined by the matrix dh which has one row per joint and each row is [theta d a alpha] and joints are assumed revolute. An optional fifth column sigma indicate revolute (sigma=0, default) or prismatic (sigma=1).


% |th|d|a|alpha|R|
% -------
% |0 0|0.281|-pi/2|false|
% |0 1.005|0|pi/2|true|
% |pi/2 0|0.6|0|true|
% |pi/2 0|0.2|pi/2|true|
% |0|0.64|0|-pi/2|true|
% |0|0|0|pi/2|true|
% |0|0.1|0|0|true|

L = XYZ_ref(:,1);
dh = [0 L 0.281 -pi/2 true ;
0 1.005 0 pi/2 false ;
pi/2 0 0.6 0 false ;
pi/2 0 0.2 pi/2 false ;
0 0.64 0 -pi/2 false ;
0 0 0 pi/2 false ;
0 0.1 0 0 false ];

R = SerialLink(dh,'qlim',[0 1;-pi pi;-pi pi;-pi pi;-pi pi;-pi pi;-pi pi]);

XYZ2 = zeros(3,Nsim);
for k=1:Nsim
Tee = R.fkine([Gantry_out_D.Data(k,2),...
                     Axis1_Out_D.Data(k,2)/180*pi,...
                     Axis2_Out_D.Data(k,2)/180*pi,...
                     Axis3_Out_D.Data(k,2)/180*pi,...
                     Axis4_Out_D.Data(k,2)/180*pi,...
                     Axis5_Out_D.Data(k,2)/180*pi,...
                     Axis6_Out_D.Data(k,2)/180*pi]);
XYZ2(:,k) = transl(Tee)';
end


figure
hold on
plot3(simOut.EndEffector_AbsPos{1}.Values.Data,simOut.EndEffector_AbsPos{2}.Values.Data,simOut.EndEffector_AbsPos{3}.Values.Data)
%scatter3(simOut.EndEffector_AbsPos{1}.Values.Data(1),simOut.EndEffector_AbsPos{2}.Values.Data(1),simOut.EndEffector_AbsPos{3}.Values.Data(1))
plot3(XYZ(1,:),XYZ(2,:),XYZ(3,:))
%scatter3(XYZ(1,1),XYZ(2,1),XYZ(3,1))
plot3(XYZ2(1,:),XYZ2(2,:),XYZ2(3,:))
%scatter3(XYZ2(1,1),XYZ2(2,1),XYZ2(3,1))
legend('NX','Remy','DH')
xlabel('x')
ylabel('y')
zlabel('z')
axis square

teach(R)

%% Save .mat file$