import solids; import tube; import graph3; import palette; size(8cm); currentprojection=perspective( camera=(13.3596389245356,8.01038090435314,14.4864483364785), up=(-0.0207054323419367,-0.00472438375047319,0.0236460907598947), target=(-1.06042550499095,2.68154529985845,0.795007562120261)); defaultpen(fontsize(6pt)); // draw coordinates and frames // axis1 is defined by z axis of TBase // axis2 is defined by z axis of TEnd void DrawFrame(transform3 TBase, transform3 TEnd, string s) { triple p1,v1,p2,v2; p1=TBase*O; v1=TBase*Z-p1; p2=TEnd*O; v2=TEnd*Z-p2; triple n=cross(v1,v2); real[][] A= { {v1.x,-v2.x,-n.x}, {v1.y,-v2.y,-n.y}, {v1.z,-v2.z,-n.z} }; triple vb=p2-p1; real[] b={vb.x,vb.y,vb.z}; // Get the extention along vector v1 and v2, // so, we can get the common normal between two axis real[] x=solve(A,b); real s1=x[0]; real s2=x[1]; // get foot of a perpendicular on both axies triple foot1=p1+s1*v1; triple foot2=p2+s2*v2; // draw two axis triple axis_a,axis_b; axis_a=p1+s1*v1*1.5; axis_b=p1-s1*v1*1.5; draw(axis_a--axis_b); axis_a=p2+s2*v2*1.5; axis_b=p2-s2*v2*1.5; draw(axis_a--axis_b); // draw "a"(common normal) draw(Label("$a_{"+s+"}$"),foot1--foot2,linewidth(1pt)); // draw the coordinates frame triple dx,dy,dz,org; real length=0.8; org=foot1; dx =length*unit(foot2-foot1); // define the x axis of the frame on "a" dz =length*unit(v1); // define the z axis which is along axis1 dy =length*unit(cross(dz,dx)); draw(Label("$X_{"+s+"}$",1,align=-dy-dz),org--(org+dx),red+linewidth(1.5pt), Arrow3(8)); draw(Label("$Y_{"+s+"}$",1,align=2dy-dz-dx),org--(org+dy), green+linewidth(1.5pt), Arrow3(8)); draw(Label("$Z_{"+s+"}$",1,align=-2dx-dy),org--(org+dz), blue+linewidth(1.5pt), Arrow3(8)); dot(Label("$O_{"+s+"}$",align =-dx-dz,black),org,black); // origin } void DrawLink(transform3 TBase, transform3 TEnd, pen objStyle,string s) { real h=1; real r=0.5; path3 generator=(0.5*r,0,h)--(r,0,h)--(r,0,0)--(0.5*r,0,0); revolution vase=revolution(O,generator,0,360); surface objSurface=surface(vase); render render=render(merge=true); // draw two cylinders draw(TBase*objSurface,objStyle,render); draw(TEnd*shift((0,0,-h))*objSurface,objStyle,render); // draw the link between two cylinders triple pStart=TBase*(0.5*h*Z); triple pEnd =TEnd*(-0.5*h*Z); triple pControl1=0.25*(pEnd-pStart)+TBase*(0,0,h); triple pControl2=-0.25*(pEnd-pStart)+TEnd*(0,0,-h); path3 p=pStart..controls pControl1 and pControl2..pEnd; draw(tube(p,scale(0.2)*unitsquare),objStyle,render); } // t1 and t2 define the starting frame and ending frame of the first link(i-1) transform3 t1=shift((0,0,1)); transform3 t2=shift((0,0,-1))*rotate(-20,Y)*shift((0,3,2)); // as, the two links were connected, so t2 is also the starting frame of link(i) // t3 defines the ending frame of link(i) transform3 t3=t2*rotate(40,Z)*shift((0,3,1.5))*rotate(-15,Y)*shift(-1.5*Z); // draw link(i-1) DrawLink(t1,t2,palegreen,"i-1"); DrawFrame(t1,t2,"i-1"); // draw link(i) DrawLink(t2,t3,lightmagenta,"i"); DrawFrame(t2,t3,"i"); // draw angle alpha, which is the angle between axis(i-1) and axis(i) triple p0=(0,0,-1); triple p1=(0,0,2.3); triple p2=shift((0,0,-1))*rotate(-20,Y)*(0,0,4); draw(p0--p2,cyan); draw("$\alpha_{i-1}$",arc(p0,p1,p2,Y,CW),ArcArrow3(3)); // draw angle theta, which is the angle between a_i and a_{i-1} transform3 tx=shift((0,0,-1))*rotate(-20,Y)*shift((0,3,0)); p0=tx*O; p1=tx*(0,3,0); p2=tx*rotate(40,Z)*(0,3,0); draw(p0--p1,cyan); draw(p0--p2,cyan); triple p1a=tx*(0,1.5,0); draw("$\theta_{i}$",arc(p0,p1a,p2),ArcArrow3(3)); // draw d_{i-1} triple org_i =t2*shift((0,0,1.5))*O; draw(Label("$d_{i}$",0.13),p0--org_i,linewidth(1pt));