Skip to content

Instantly share code, notes, and snippets.

@jgillis
Created December 3, 2021 01:02
Show Gist options
  • Save jgillis/27152fec37ae6b476e5d8d6bfe639f8d to your computer and use it in GitHub Desktop.
Save jgillis/27152fec37ae6b476e5d8d6bfe639f8d to your computer and use it in GitHub Desktop.
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$
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment