LZScene

1
// this unit isn't used at all at the moment, just putting down some ideas
2
// for serial-link manipulators
3
unit GLJoints;
4

5
interface
6

7
uses
8
  GLScene,
9
  GLVectorGeometry;
10

11
type
12

13
  (*
14
    //
15
    //                         Joint(Z1)      Joint(Z3)
16
    //                          / \            |_| (Wrist)
17
    //                         / \/\           //
18
    //                        / / \ \         //
19
    //                       / /   \ \(1)    //(2)
20
    //             Link(0)  / /     \ \     //
21
    //                     / /       \ \   //
22
    //                    / /         \ \ //
23
    //                   / /           \/\/
24
    //         Joint(Z0)/ /             \/
25
    //               |------|          Joint(Z2)
26
    //               | Base |
27
    //     --------------------------------------------
28
    //     X(n) = Common normal between joint axis Z(n-1) & Z(n)
29
  *)
30

31
  TGLBaseJoint = class(TObject)
32
  end;
33

34
  TGLBaseLink = class(TObject)
35
  end;
36

37
  TGLJoint = class(TGLBaseJoint)
38
    Link1: TGLBaseLink; // if Link1 is nil, assumed to be base
39
    Link2: TGLBaseLink; // if Link2 is nil, assumed to be wrist
40
    // Object1:TGLBaseSceneObject;
41
    // Object2:TGLBaseSceneObject;
42
  end;
43

44
  // Links are mainly for used for Serial-Link manipulators
45

46
  // Direct & Inverse Kinematics algorithms are planned
47
  TGLLink = class(TGLBaseLink)
48
    // Link Parameters
49
    fLinkLength: Real; // Length of common normal which is orthogonal to both
50
    // joint axes Z[n-1] and Z[n] (a.k.a.  L)
51
    fTwistAngle: Real; // Twist angle between joint axes, if joint frames were
52
    // coincident (a.k.a.  Theta?)
53
    fLinkAngle: Real;
54
    // Angle between common normals X[n-1] and X[n] (a.k.a.  Alpha?)
55
    fLinkDistance: Real;
56
    // Distance along joint axis Z[n-1] between intersection
57
    // points of common normals X[n-1] and X[n] (a.k.a.  d)
58
(*
59
    //
60
    // A-Matrix
61
    // [ Cos(Theta)   -Sin(Theta)*Cos(Alpha)    Sin(Theta)*Sin(Alpha)   L*Cos(Theta) ]
62
    // [ Sin(Theta)    Cos(Theta)*cos(Alpha)   -Cos(Theta)*Sin(Alpha)   L*Sin(Theta) ]
63
    // [    0            Sin(Alpha)                   Cos(Alpha)           d         ]
64
    // [    0               0                            0                 1         ]
65
    //
66
*)
67
    A: TMatrix;
68
    constructor Create(LinkLength, TwistAngle, LinkAngle, LinkDistance: Real);
69
    // constructor Create();virtual;
70
  end;
71

72
  // see html file for description of the different links
73
(*
74
  // Type 1 Two-Link Manipulator
75
  //
76
  // _ ________
77
  // / \        \
78
  // / \/________/
79
  // /  /
80
  // /  /
81
  // /  /
82
  // /  /
83
  // /  /
84
  // \_/
85
  //
86
*)
87
  TGLType1Link = class(TGLLink)
88
    Length: Real; // fixed
89
    Angle: Real; // variable
90
(*
91
    // A-Matrix
92
    // [ C1   -S1    0    L1C1 ]
93
    // [ S1    C1    0    L1S1 ]
94
    // [  0     0    1      0  ]
95
    // [  0     0    0      1  ]
96
    //
97
*)
98
    constructor Create(Length, Angle: Real);
99
  end;
100
(*
101
  // Type 2 Two-Link Manipulator
102
  // ___
103
  // /   \___________________
104
  // | + |___________________|
105
  // \   /
106
  // | |
107
  // | |
108
  // | |
109
  // | |
110
  // | |
111
  // | |
112
  // __|_|__
113
  // |___.___|
114
  //
115
*)
116
  TGLType2Link = class(TGLLink)
117
    Length: Real; // fixed
118
    Angle: Real; // variable
119
    constructor Create(Length, Angle: Real);
120
  end;
121
(*
122
  // Type 3 Two-Link Manipulator
123
  // _______
124
  // _________|       |__
125
  // <-|_________|       |__|->
126
  // |__   __|
127
  // | |
128
  // | |
129
  // | |
130
  // | |
131
  // | |
132
  // _|_|_
133
  // |__.__|
134
*)
135
  TGLType3Link = class(TGLLink)
136
    Length: Real; // fixed
137
    Angle: Real; // variable
138
(*
139
    // A-Matrix
140
    // [  1    0    0    0  ]
141
    // [  0    1    0    0  ]
142
    // [  0    0    1    d2 ]
143
    // [  0    0    0    1  ]
144
    //
145
*)
146
    constructor Create(Length, Angle: Real);
147
  end;
148

149
  TGLType4Link = class(TGLLink)
150
    Length: Real; // fixed
151
    Angle: Real; // variable
152
    constructor Create(Length, Angle: Real);
153
  end;
154
(*
155
  // Type 5 Two-Link Manipulator
156
  //
157
  // _
158
  // ________|_|__
159
  // <-|_____________|->
160
  // | |
161
  // | |
162
  // | |
163
  // | |
164
  // | |
165
  // __| |__
166
  // |__| |__|
167
  // |_|
168
  // |
169
  // V
170
  //
171
*)
172
  TGLType5Link = class(TGLLink)
173
    Length: Real; // variable
174
    constructor Create(Length, Angle: Real);
175
  end;
176

177
  TGLType6Link = class(TGLLink)
178
    Length: Real; // fixed
179
    Angle: Real; // variable
180
    constructor Create(Length, Angle: Real);
181
  end;
182

183
  TGLType7Link = class(TGLLink)
184
    Length: Real; // fixed
185
    Angle: Real; // variable
186
    constructor Create(Length, Angle: Real);
187
  end;
188

189
  TGLType8Link = class(TGLLink)
190
    Length: Real; // variable
191
    constructor Create(Length, Angle: Real);
192
  end;
193

194
  TGLPrismaticJoint = class(TGLJoint)
195
  end;
196

197
  TGLRevoluteJoint = class(TGLJoint)
198
  end;
199

200
  TGLBallAndSocketJoint = class(TGLJoint)
201
  end;
202

203
//======================================================================
204
implementation
205
//======================================================================
206

207
constructor TGLLink.Create(LinkLength, TwistAngle, LinkAngle,
208
  LinkDistance: Real);
209
begin
210
  fLinkLength := LinkLength;
211
  fTwistAngle := TwistAngle;
212
  fLinkAngle := LinkAngle;
213
  fLinkDistance := LinkDistance;
214
end;
215

216
constructor TGLType1Link.Create(Length, Angle: Real);
217
begin
218
  inherited Create(Length, 0, Angle, 0);
219
end;
220

221
constructor TGLType2Link.Create(Length, Angle: Real);
222
begin
223
  inherited Create(Length, 0, Angle, 0);
224
end;
225

226
constructor TGLType3Link.Create(Length, Angle: Real);
227
begin
228
  inherited Create(Length, 0, Angle, 0);
229
end;
230

231
constructor TGLType4Link.Create(Length, Angle: Real);
232
begin
233
  inherited Create(Length, 0, Angle, 0);
234
end;
235

236
constructor TGLType5Link.Create(Length, Angle: Real);
237
begin
238
  inherited Create(Length, 0, Angle, 0);
239
end;
240

241
constructor TGLType6Link.Create(Length, Angle: Real);
242
begin
243
  inherited Create(Length, 0, Angle, 0);
244
end;
245

246
constructor TGLType7Link.Create(Length, Angle: Real);
247
begin
248
  inherited Create(Length, 0, Angle, 0);
249
end;
250

251
constructor TGLType8Link.Create(Length, Angle: Real);
252
begin
253
  inherited Create(Length, 0, Angle, 0);
254
end;
255

256
end.
257