robot.tool.t = [0, 0, 0];

t1 = robot.fkine(q1).t;
t2 = robot.fkine(q2).t;
t3 = robot.fkine(q3).t;
t4 = robot.fkine(q4).t;
t5 = robot.fkine(q5).t;
t6 = robot.fkine(q6).t;

RR = [R1 - R2; R2 - R3; R3 - R4];
PP = [t2 - t1; t3 - t2; t4 - t3];

ox = R4 \ (t5 - t4) / norm(t5 - t4);
oy = R4 \ (t6 - t4) / norm(t6 - t4);
oz = cross(ox, oy);

p_tool = RR \ PP;
R_tool = [ox, oy, oz];

disp("Tool end position");
disp(p_tool);
disp("Tool end attitude matrix");
disp(R_tool);