1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
|
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+1e-5))*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));
|
