I want to modify this Mathematica file in order to plot also the others joint frames  in the robotplot and not only the first one:
In[4]:= dhParameters={
{q1,d1,a1,-\[Pi]/2},
{q2-\[Pi]/2,0,a2,0},
{q3,0,a3,-\[Pi]/2},
{q4,d4,0,\[Pi]/2},
{q5+\[Pi],0,0,\[Pi]/2},
{q6,d6,0,0}
};
params={a1->0.175,a2->1.095,a3->0.175,d1->0.495,d4->1.270,d6->0.135};
qlim=
{
{(-180*Pi)/180,(180*Pi)/180},
{(-90*Pi)/180,(150*Pi)/180},
{(-180*Pi)/180,(75*Pi)/180},
{(-400*Pi)/180,(400*Pi)/180},
{(-125*Pi)/180,(120*Pi)/180},
{(-400*Pi)/180,(400*Pi)/180}
};
DH PARAMS - Anthropomorphic arm
(*dhParameters={
{\[Theta]1,0,0,\[Pi]/2},
{\[Theta]2,0,a2,0},
{\[Theta]3,0,a3,0}
};*)
In[8]:= (*dhParameters={
{q1 + \[Theta]1 ,d1,a1,\[Alpha]1},
{q2+\[Theta]2,d2,a2,\[Alpha]2},
{q3+\[Theta]3,d3,a3,\[Alpha]3},
{q4+\[Theta]4,d4,a4,\[Alpha]4},
{q5+\[Theta]5,d5,a5,\[Alpha]5},
{q6+\[Theta]6,d6,a6,\[Alpha]6}
};*)
DH PARAMS - IRB 7600 325
(*params={a1->0.41,a2->1.075,a3->0.165,d1->0.78,d4->1.556,d6->0.25};
qlim=
{
{(-180*Pi)/180,(180*Pi)/180},
{(-15*Pi)/180,(135*Pi)/180},
{(-65*Pi)/180,(220*Pi)/180},
{(0*Pi)/180,(360*Pi)/180},
{(-130*Pi)/180,(130*Pi)/180},
{(0*Pi)/180,(360*Pi)/180}
};*)

DH PARAMS - IRB 4600 40



In[7]:= zeros = {q1->0,q2->0,q3->0,q4->0,q5->0,q6->0}
Out[7]= {q1->0,q2->0,q3->0,q4->0,q5->0,q6->0}
In[8]:= (*Define the transformation matrix using DH parameters*)
dhTransform[{theta_,d_,a_,alpha_}]:={{Cos[theta],-Sin[theta] Cos[alpha],Sin[theta] Sin[alpha],a Cos[theta]},{Sin[theta],Cos[theta] Cos[alpha],-Cos[theta] Sin[alpha],a Sin[theta]},{0,Sin[alpha],Cos[alpha],d},{0,0,0,1}};
MatrixForm@dhTransform[{\[Theta],d,a,\[Alpha]}]
Out[9]//MatrixForm= (Cos[\[Theta]]	-Cos[\[Alpha]] Sin[\[Theta]]	Sin[\[Alpha]] Sin[\[Theta]]	a Cos[\[Theta]]
Sin[\[Theta]]	Cos[\[Alpha]] Cos[\[Theta]]	-Cos[\[Theta]] Sin[\[Alpha]]	a Sin[\[Theta]]
0	Sin[\[Alpha]]	Cos[\[Alpha]]	d
0	0	0	1

)
Compute transformation matrices
In[109]:= (*Compute the overall transformation matrix T_0_6*)
A06=IdentityMatrix[4];
oiRF0={{0,0,0}};
zi={{0,0,0}};
Z={{0},{0},{1}};
P={{0},{0},{0}};
A0iall={A06};
Aim1iall={A06};
Do[
{
Ai=Simplify[dhTransform[dhParameters[[i]]]],
A06=Simplify[A06.Ai],
A0iall=Append[A0iall,A06],
oiRF0=Append[oiRF0,A06[[1;;3,4]]],
zi=Append[zi,A06[[1;;3,3]]],
Aim1iall=Append[Aim1iall,Ai],
Z=ArrayFlatten[{{Z,Transpose[{A06[[1;;3,3]]}]}}],
P= ArrayFlatten[{{P,Transpose[{A06[[1;;3,4]]}]}}]
},
{i,1,Length[dhParameters]}
];
A06=FullSimplify[A06];
In[104]:= MatrixForm[Aim1iall[[6]]] //Simplify
Out[104]//MatrixForm= (-Cos[q5]	0	-Sin[q5]	0
-Sin[q5]	0	Cos[q5]	0
0	1	0	0
0	0	0	1

)
In[103]:= MatrixForm[A0iall[[6]]]//FullSimplify
Out[103]//MatrixForm= (-Cos[q5] Sin[q1] Sin[q4]-Cos[q1] (Cos[q4] Cos[q5] Sin[q2+q3]+Cos[q2+q3] Sin[q5])	-Cos[q4] Sin[q1]+Cos[q1] Sin[q2+q3] Sin[q4]	Cos[q1] Cos[q2+q3] Cos[q5]-(Cos[q1] Cos[q4] Sin[q2+q3]+Sin[q1] Sin[q4]) Sin[q5]	Cos[q1] (a1+d4 Cos[q2+q3]+a2 Sin[q2]+a3 Sin[q2+q3])
-Cos[q4] Cos[q5] Sin[q1] Sin[q2+q3]+Cos[q1] Cos[q5] Sin[q4]-Cos[q2+q3] Sin[q1] Sin[q5]	Cos[q1] Cos[q4]+Sin[q1] Sin[q2+q3] Sin[q4]	Cos[q2+q3] Cos[q5] Sin[q1]+(-Cos[q4] Sin[q1] Sin[q2+q3]+Cos[q1] Sin[q4]) Sin[q5]	Sin[q1] (a1+d4 Cos[q2+q3]+a2 Sin[q2]+a3 Sin[q2+q3])
-Cos[q2+q3] Cos[q4] Cos[q5]+Sin[q2+q3] Sin[q5]	Cos[q2+q3] Sin[q4]	-Cos[q5] Sin[q2+q3]-Cos[q2+q3] Cos[q4] Sin[q5]	d1+a2 Cos[q2]+a3 Cos[q2+q3]-d4 Sin[q2+q3]
0	0	0	1

)
Extract position and orientation of  the TCP from the transformation matrix
In[120]:= pTCP=A06[[1;;3,4]]; MatrixForm@pTCP
RTCP=A06[[1;;3,1;;3]];MatrixForm@RTCP
Out[120]//MatrixForm= (-d6 Sin[q1] Sin[q4] Sin[q5]+Cos[q1] (a1+Cos[q2+q3] (d4+d6 Cos[q5])+a2 Sin[q2]+Sin[q2+q3] (a3-d6 Cos[q4] Sin[q5]))
d6 Cos[q1] Sin[q4] Sin[q5]+Sin[q1] (a1+Cos[q2+q3] (d4+d6 Cos[q5])+a2 Sin[q2]+Sin[q2+q3] (a3-d6 Cos[q4] Sin[q5]))
d1+a2 Cos[q2]-(d4+d6 Cos[q5]) Sin[q2+q3]+Cos[q2+q3] (a3-d6 Cos[q4] Sin[q5])

)
Out[121]//MatrixForm= (-Sin[q1] (Cos[q5] Cos[q6] Sin[q4]+Cos[q4] Sin[q6])+Cos[q1] (-Cos[q6] (Cos[q4] Cos[q5] Sin[q2+q3]+Cos[q2+q3] Sin[q5])+Sin[q2+q3] Sin[q4] Sin[q6])	Cos[q5] Sin[q1] Sin[q4] Sin[q6]+Cos[q4] (-Cos[q6] Sin[q1]+Cos[q1] Cos[q5] Sin[q2+q3] Sin[q6])+Cos[q1] (Cos[q6] Sin[q2+q3] Sin[q4]+Cos[q2+q3] Sin[q5] Sin[q6])	Cos[q1] Cos[q2+q3] Cos[q5]-(Cos[q1] Cos[q4] Sin[q2+q3]+Sin[q1] Sin[q4]) Sin[q5]
-Cos[q6] (Cos[q4] Cos[q5] Sin[q1] Sin[q2+q3]-Cos[q1] Cos[q5] Sin[q4]+Cos[q2+q3] Sin[q1] Sin[q5])+(Cos[q1] Cos[q4]+Sin[q1] Sin[q2+q3] Sin[q4]) Sin[q6]	Cos[q1] (Cos[q4] Cos[q6]-Cos[q5] Sin[q4] Sin[q6])+Sin[q1] (Cos[q6] Sin[q2+q3] Sin[q4]+(Cos[q4] Cos[q5] Sin[q2+q3]+Cos[q2+q3] Sin[q5]) Sin[q6])	Cos[q2+q3] Cos[q5] Sin[q1]+(-Cos[q4] Sin[q1] Sin[q2+q3]+Cos[q1] Sin[q4]) Sin[q5]
Cos[q6] Sin[q2+q3] Sin[q5]+Cos[q2+q3] (-Cos[q4] Cos[q5] Cos[q6]+Sin[q4] Sin[q6])	-Sin[q2+q3] Sin[q5] Sin[q6]+Cos[q2+q3] (Cos[q6] Sin[q4]+Cos[q4] Cos[q5] Sin[q6])	-Cos[q5] Sin[q2+q3]-Cos[q2+q3] Cos[q4] Sin[q5]

)
Position of the TCP in zero joint configuration
In[162]:= pTCP/.params/.zeros
Out[162]= {1.58,0.,1.765}
In[164]:= RTCP/.params/.zeros //MatrixForm
Out[164]//MatrixForm= (0	0	1
0	1	0
-1	0	0

)
Robotplot
In[138]:= RobotPlot=ListLinePlot3D[{oiRF0[[1]],oiRF0[[2]],oiRF0[[3]],oiRF0[[4]], oiRF0[[5]],oiRF0[[6]]}/.params/.zeros,BoxRatios->Automatic,PlotRange->Automatic];
In[165]:= labels={"X0","Y0","Z0"};
For[i=1,i<4,i++,u={0,0,0};
(*initializing u as a null 3 elements vector*)
u[[i]]=0.2;
(*Setting the i-th element of u equal to 1,to have a unit vector*)
RF0arrow[i]=Graphics3D[
{Thickness[0.0010],Red,Arrowheads[0.04],Text[labels[[i]],2 u+0.07],Arrow[{{0,0,0},2 u}]},
Axes->True];
]
Show[{RobotPlot/.params/.zeros,RF0arrow[1],RF0arrow[2],RF0arrow[3]}]