urdf-loader 0.10.4 → 0.11.0

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
package/umd/URDFLoader.js CHANGED
@@ -26,1062 +26,1055 @@
26
26
 
27
27
  var THREE__namespace = /*#__PURE__*/_interopNamespace(THREE);
28
28
 
29
- class URDFBase extends THREE.Object3D {
30
-
31
- constructor(...args) {
32
-
33
- super(...args);
34
- this.urdfNode = null;
35
- this.urdfName = '';
36
-
37
- }
38
-
39
- copy(source, recursive) {
40
-
41
- super.copy(source, recursive);
42
-
43
- this.urdfNode = source.urdfNode;
44
- this.urdfName = source.urdfName;
45
-
46
- return this;
47
-
48
- }
49
-
50
- }
51
-
52
- class URDFCollider extends URDFBase {
53
-
54
- constructor(...args) {
55
-
56
- super(...args);
57
- this.isURDFCollider = true;
58
- this.type = 'URDFCollider';
59
-
60
- }
61
-
62
- }
63
-
64
- class URDFVisual extends URDFBase {
65
-
66
- constructor(...args) {
67
-
68
- super(...args);
69
- this.isURDFVisual = true;
70
- this.type = 'URDFVisual';
71
-
72
- }
73
-
74
- }
75
-
76
- class URDFLink extends URDFBase {
77
-
78
- constructor(...args) {
79
-
80
- super(...args);
81
- this.isURDFLink = true;
82
- this.type = 'URDFLink';
83
-
84
- }
85
-
86
- }
87
-
88
- class URDFJoint extends URDFBase {
89
-
90
- get jointType() {
91
-
92
- return this._jointType;
93
-
94
- }
95
-
96
- set jointType(v) {
97
-
98
- if (this.jointType === v) return;
99
- this._jointType = v;
100
- this.matrixWorldNeedsUpdate = true;
101
- switch (v) {
102
-
103
- case 'fixed':
104
- this.jointValue = [];
105
- break;
106
-
107
- case 'continuous':
108
- case 'revolute':
109
- case 'prismatic':
110
- this.jointValue = new Array(1).fill(0);
111
- break;
112
-
113
- case 'planar':
114
- this.jointValue = new Array(2).fill(0);
115
- break;
116
-
117
- case 'floating':
118
- this.jointValue = new Array(6).fill(0);
119
- break;
120
-
121
- }
122
-
123
- }
124
-
125
- get angle() {
126
-
127
- return this.jointValue[0];
128
-
129
- }
130
-
131
- constructor(...args) {
132
-
133
- super(...args);
134
-
135
- this.isURDFJoint = true;
136
- this.type = 'URDFJoint';
137
-
138
- this.jointValue = null;
139
- this.jointType = 'fixed';
140
- this.axis = new THREE.Vector3(1, 0, 0);
141
- this.limit = { lower: 0, upper: 0 };
142
- this.ignoreLimits = false;
143
-
144
- this.origPosition = null;
145
- this.origQuaternion = null;
146
-
147
- this.mimicJoints = [];
148
-
149
- }
150
-
151
- /* Overrides */
152
- copy(source, recursive) {
153
-
154
- super.copy(source, recursive);
155
-
156
- this.jointType = source.jointType;
157
- this.axis = source.axis.clone();
158
- this.limit.lower = source.limit.lower;
159
- this.limit.upper = source.limit.upper;
160
- this.ignoreLimits = false;
161
-
162
- this.jointValue = [...source.jointValue];
163
-
164
- this.origPosition = source.origPosition ? source.origPosition.clone() : null;
165
- this.origQuaternion = source.origQuaternion ? source.origQuaternion.clone() : null;
166
-
167
- this.mimicJoints = [...source.mimicJoints];
168
-
169
- return this;
170
-
171
- }
172
-
173
- /* Public Functions */
174
- setJointValue(...values) {
175
-
176
- values = values.map(v => parseFloat(v));
177
-
178
- if (!this.origPosition || !this.origQuaternion) {
179
-
180
- this.origPosition = this.position.clone();
181
- this.origQuaternion = this.quaternion.clone();
182
-
183
- }
184
-
185
- let didUpdate = false;
186
-
187
- this.mimicJoints.forEach(joint => {
188
-
189
- didUpdate = joint.updateFromMimickedJoint(...values) || didUpdate;
190
-
191
- });
192
-
193
- switch (this.jointType) {
194
-
195
- case 'fixed': {
196
-
197
- return didUpdate;
198
-
199
- }
200
- case 'continuous':
201
- case 'revolute': {
202
-
203
- let angle = values[0];
204
- if (angle == null) return didUpdate;
205
- if (angle === this.jointValue[0]) return didUpdate;
206
-
207
- if (!this.ignoreLimits && this.jointType === 'revolute') {
208
-
209
- angle = Math.min(this.limit.upper, angle);
210
- angle = Math.max(this.limit.lower, angle);
211
-
212
- }
213
-
214
- this.quaternion
215
- .setFromAxisAngle(this.axis, angle)
216
- .premultiply(this.origQuaternion);
217
-
218
- if (this.jointValue[0] !== angle) {
219
-
220
- this.jointValue[0] = angle;
221
- this.matrixWorldNeedsUpdate = true;
222
- return true;
223
-
224
- } else {
225
-
226
- return didUpdate;
227
-
228
- }
229
-
230
- }
231
-
232
- case 'prismatic': {
233
-
234
- let pos = values[0];
235
- if (pos == null) return didUpdate;
236
- if (pos === this.jointValue[0]) return didUpdate;
237
-
238
- if (!this.ignoreLimits) {
239
-
240
- pos = Math.min(this.limit.upper, pos);
241
- pos = Math.max(this.limit.lower, pos);
242
-
243
- }
244
-
245
- this.position.copy(this.origPosition);
246
- this.position.addScaledVector(this.axis, pos);
247
-
248
- if (this.jointValue[0] !== pos) {
249
-
250
- this.jointValue[0] = pos;
251
- this.matrixWorldNeedsUpdate = true;
252
- return true;
253
-
254
- } else {
255
-
256
- return didUpdate;
257
-
258
- }
259
-
260
- }
261
-
262
- case 'floating':
263
- case 'planar':
264
- // TODO: Support these joint types
265
- console.warn(`'${ this.jointType }' joint not yet supported`);
266
-
267
- }
268
-
269
- return didUpdate;
270
-
271
- }
272
-
273
- }
274
-
275
- class URDFMimicJoint extends URDFJoint {
276
-
277
- constructor(...args) {
278
-
279
- super(...args);
280
- this.type = 'URDFMimicJoint';
281
- this.mimicJoint = null;
282
- this.offset = 0;
283
- this.multiplier = 1;
284
-
285
- }
286
-
287
- updateFromMimickedJoint(...values) {
288
-
289
- const modifiedValues = values.map(x => x * this.multiplier + this.offset);
290
- return super.setJointValue(...modifiedValues);
291
-
292
- }
293
-
294
- /* Overrides */
295
- setJointValue(...values) {
296
-
297
- console.warn(`URDFMimicJoint: Setting the joint value of mimic joint "${ this.urdfName }" will cause it to be out of sync.`);
298
- return super.setJointValue(...values);
299
- }
300
-
301
- /* Overrides */
302
- copy(source, recursive) {
303
-
304
- super.copy(source, recursive);
305
-
306
- this.mimicJoint = source.mimicJoint;
307
- this.offset = source.offset;
308
- this.multiplier = source.multiplier;
309
-
310
- return this;
311
-
312
- }
313
-
314
- }
315
-
316
- class URDFRobot extends URDFLink {
317
-
318
- constructor(...args) {
319
-
320
- super(...args);
321
- this.isURDFRobot = true;
322
- this.urdfNode = null;
323
-
324
- this.urdfRobotNode = null;
325
- this.robotName = null;
326
-
327
- this.links = null;
328
- this.joints = null;
329
- this.colliders = null;
330
- this.visual = null;
331
- this.frames = null;
332
-
333
- }
334
-
335
- copy(source, recursive) {
336
-
337
- super.copy(source, recursive);
338
-
339
- this.urdfRobotNode = source.urdfRobotNode;
340
- this.robotName = source.robotName;
341
-
342
- this.links = {};
343
- this.joints = {};
344
- this.colliders = {};
345
- this.visual = {};
346
-
347
- this.traverse(c => {
348
-
349
- if (c.isURDFJoint && c.urdfName in source.joints) {
350
-
351
- this.joints[c.urdfName] = c;
352
-
353
- }
354
-
355
- if (c.isURDFLink && c.urdfName in source.links) {
356
-
357
- this.links[c.urdfName] = c;
358
-
359
- }
360
-
361
- if (c.isURDFCollider && c.urdfName in source.colliders) {
362
-
363
- this.colliders[c.urdfName] = c;
364
-
365
- }
366
-
367
- if (c.isURDFVisual && c.urdfName in source.visual) {
368
-
369
- this.visual[c.urdfName] = c;
370
-
371
- }
372
-
373
- });
374
-
375
- this.frames = {
376
- ...this.colliders,
377
- ...this.visual,
378
- ...this.links,
379
- ...this.joints,
380
- };
381
-
382
- return this;
383
-
384
- }
385
-
386
- getFrame(name) {
387
-
388
- return this.frames[name];
389
-
390
- }
391
-
392
- setJointValue(jointName, ...angle) {
393
-
394
- const joint = this.joints[jointName];
395
- if (joint) {
396
-
397
- return joint.setJointValue(...angle);
398
-
399
- }
400
-
401
- return false;
402
- }
403
-
404
- setJointValues(values) {
405
-
406
- let didChange = false;
407
- for (const name in values) {
408
-
409
- const value = values[name];
410
- if (Array.isArray(value)) {
411
-
412
- didChange = this.setJointValue(name, ...value) || didChange;
413
-
414
- } else {
415
-
416
- didChange = this.setJointValue(name, value) || didChange;
417
-
418
- }
419
-
420
- }
421
-
422
- return didChange;
423
-
424
- }
425
-
29
+ class URDFBase extends THREE.Object3D {
30
+
31
+ constructor(...args) {
32
+
33
+ super(...args);
34
+ this.urdfNode = null;
35
+ this.urdfName = '';
36
+
37
+ }
38
+
39
+ copy(source, recursive) {
40
+
41
+ super.copy(source, recursive);
42
+
43
+ this.urdfNode = source.urdfNode;
44
+ this.urdfName = source.urdfName;
45
+
46
+ return this;
47
+
48
+ }
49
+
50
+ }
51
+
52
+ class URDFCollider extends URDFBase {
53
+
54
+ constructor(...args) {
55
+
56
+ super(...args);
57
+ this.isURDFCollider = true;
58
+ this.type = 'URDFCollider';
59
+
60
+ }
61
+
62
+ }
63
+
64
+ class URDFVisual extends URDFBase {
65
+
66
+ constructor(...args) {
67
+
68
+ super(...args);
69
+ this.isURDFVisual = true;
70
+ this.type = 'URDFVisual';
71
+
72
+ }
73
+
74
+ }
75
+
76
+ class URDFLink extends URDFBase {
77
+
78
+ constructor(...args) {
79
+
80
+ super(...args);
81
+ this.isURDFLink = true;
82
+ this.type = 'URDFLink';
83
+
84
+ }
85
+
86
+ }
87
+
88
+ class URDFJoint extends URDFBase {
89
+
90
+ get jointType() {
91
+
92
+ return this._jointType;
93
+
94
+ }
95
+
96
+ set jointType(v) {
97
+
98
+ if (this.jointType === v) return;
99
+ this._jointType = v;
100
+ this.matrixWorldNeedsUpdate = true;
101
+ switch (v) {
102
+
103
+ case 'fixed':
104
+ this.jointValue = [];
105
+ break;
106
+
107
+ case 'continuous':
108
+ case 'revolute':
109
+ case 'prismatic':
110
+ this.jointValue = new Array(1).fill(0);
111
+ break;
112
+
113
+ case 'planar':
114
+ this.jointValue = new Array(2).fill(0);
115
+ break;
116
+
117
+ case 'floating':
118
+ this.jointValue = new Array(6).fill(0);
119
+ break;
120
+
121
+ }
122
+
123
+ }
124
+
125
+ get angle() {
126
+
127
+ return this.jointValue[0];
128
+
129
+ }
130
+
131
+ constructor(...args) {
132
+
133
+ super(...args);
134
+
135
+ this.isURDFJoint = true;
136
+ this.type = 'URDFJoint';
137
+
138
+ this.jointValue = null;
139
+ this.jointType = 'fixed';
140
+ this.axis = new THREE.Vector3(1, 0, 0);
141
+ this.limit = { lower: 0, upper: 0 };
142
+ this.ignoreLimits = false;
143
+
144
+ this.origPosition = null;
145
+ this.origQuaternion = null;
146
+
147
+ this.mimicJoints = [];
148
+
149
+ }
150
+
151
+ /* Overrides */
152
+ copy(source, recursive) {
153
+
154
+ super.copy(source, recursive);
155
+
156
+ this.jointType = source.jointType;
157
+ this.axis = source.axis.clone();
158
+ this.limit.lower = source.limit.lower;
159
+ this.limit.upper = source.limit.upper;
160
+ this.ignoreLimits = false;
161
+
162
+ this.jointValue = [...source.jointValue];
163
+
164
+ this.origPosition = source.origPosition ? source.origPosition.clone() : null;
165
+ this.origQuaternion = source.origQuaternion ? source.origQuaternion.clone() : null;
166
+
167
+ this.mimicJoints = [...source.mimicJoints];
168
+
169
+ return this;
170
+
171
+ }
172
+
173
+ /* Public Functions */
174
+ setJointValue(...values) {
175
+
176
+ values = values.map(v => parseFloat(v));
177
+
178
+ if (!this.origPosition || !this.origQuaternion) {
179
+
180
+ this.origPosition = this.position.clone();
181
+ this.origQuaternion = this.quaternion.clone();
182
+
183
+ }
184
+
185
+ let didUpdate = false;
186
+
187
+ this.mimicJoints.forEach(joint => {
188
+
189
+ didUpdate = joint.updateFromMimickedJoint(...values) || didUpdate;
190
+
191
+ });
192
+
193
+ switch (this.jointType) {
194
+
195
+ case 'fixed': {
196
+
197
+ return didUpdate;
198
+
199
+ }
200
+ case 'continuous':
201
+ case 'revolute': {
202
+
203
+ let angle = values[0];
204
+ if (angle == null) return didUpdate;
205
+ if (angle === this.jointValue[0]) return didUpdate;
206
+
207
+ if (!this.ignoreLimits && this.jointType === 'revolute') {
208
+
209
+ angle = Math.min(this.limit.upper, angle);
210
+ angle = Math.max(this.limit.lower, angle);
211
+
212
+ }
213
+
214
+ this.quaternion
215
+ .setFromAxisAngle(this.axis, angle)
216
+ .premultiply(this.origQuaternion);
217
+
218
+ if (this.jointValue[0] !== angle) {
219
+
220
+ this.jointValue[0] = angle;
221
+ this.matrixWorldNeedsUpdate = true;
222
+ return true;
223
+
224
+ } else {
225
+
226
+ return didUpdate;
227
+
228
+ }
229
+
230
+ }
231
+
232
+ case 'prismatic': {
233
+
234
+ let pos = values[0];
235
+ if (pos == null) return didUpdate;
236
+ if (pos === this.jointValue[0]) return didUpdate;
237
+
238
+ if (!this.ignoreLimits) {
239
+
240
+ pos = Math.min(this.limit.upper, pos);
241
+ pos = Math.max(this.limit.lower, pos);
242
+
243
+ }
244
+
245
+ this.position.copy(this.origPosition);
246
+ this.position.addScaledVector(this.axis, pos);
247
+
248
+ if (this.jointValue[0] !== pos) {
249
+
250
+ this.jointValue[0] = pos;
251
+ this.matrixWorldNeedsUpdate = true;
252
+ return true;
253
+
254
+ } else {
255
+
256
+ return didUpdate;
257
+
258
+ }
259
+
260
+ }
261
+
262
+ case 'floating':
263
+ case 'planar':
264
+ // TODO: Support these joint types
265
+ console.warn(`'${ this.jointType }' joint not yet supported`);
266
+
267
+ }
268
+
269
+ return didUpdate;
270
+
271
+ }
272
+
273
+ }
274
+
275
+ class URDFMimicJoint extends URDFJoint {
276
+
277
+ constructor(...args) {
278
+
279
+ super(...args);
280
+ this.type = 'URDFMimicJoint';
281
+ this.mimicJoint = null;
282
+ this.offset = 0;
283
+ this.multiplier = 1;
284
+
285
+ }
286
+
287
+ updateFromMimickedJoint(...values) {
288
+
289
+ const modifiedValues = values.map(x => x * this.multiplier + this.offset);
290
+ return super.setJointValue(...modifiedValues);
291
+
292
+ }
293
+
294
+ /* Overrides */
295
+ copy(source, recursive) {
296
+
297
+ super.copy(source, recursive);
298
+
299
+ this.mimicJoint = source.mimicJoint;
300
+ this.offset = source.offset;
301
+ this.multiplier = source.multiplier;
302
+
303
+ return this;
304
+
305
+ }
306
+
307
+ }
308
+
309
+ class URDFRobot extends URDFLink {
310
+
311
+ constructor(...args) {
312
+
313
+ super(...args);
314
+ this.isURDFRobot = true;
315
+ this.urdfNode = null;
316
+
317
+ this.urdfRobotNode = null;
318
+ this.robotName = null;
319
+
320
+ this.links = null;
321
+ this.joints = null;
322
+ this.colliders = null;
323
+ this.visual = null;
324
+ this.frames = null;
325
+
326
+ }
327
+
328
+ copy(source, recursive) {
329
+
330
+ super.copy(source, recursive);
331
+
332
+ this.urdfRobotNode = source.urdfRobotNode;
333
+ this.robotName = source.robotName;
334
+
335
+ this.links = {};
336
+ this.joints = {};
337
+ this.colliders = {};
338
+ this.visual = {};
339
+
340
+ this.traverse(c => {
341
+
342
+ if (c.isURDFJoint && c.urdfName in source.joints) {
343
+
344
+ this.joints[c.urdfName] = c;
345
+
346
+ }
347
+
348
+ if (c.isURDFLink && c.urdfName in source.links) {
349
+
350
+ this.links[c.urdfName] = c;
351
+
352
+ }
353
+
354
+ if (c.isURDFCollider && c.urdfName in source.colliders) {
355
+
356
+ this.colliders[c.urdfName] = c;
357
+
358
+ }
359
+
360
+ if (c.isURDFVisual && c.urdfName in source.visual) {
361
+
362
+ this.visual[c.urdfName] = c;
363
+
364
+ }
365
+
366
+ });
367
+
368
+ this.frames = {
369
+ ...this.colliders,
370
+ ...this.visual,
371
+ ...this.links,
372
+ ...this.joints,
373
+ };
374
+
375
+ return this;
376
+
377
+ }
378
+
379
+ getFrame(name) {
380
+
381
+ return this.frames[name];
382
+
383
+ }
384
+
385
+ setJointValue(jointName, ...angle) {
386
+
387
+ const joint = this.joints[jointName];
388
+ if (joint) {
389
+
390
+ return joint.setJointValue(...angle);
391
+
392
+ }
393
+
394
+ return false;
395
+ }
396
+
397
+ setJointValues(values) {
398
+
399
+ let didChange = false;
400
+ for (const name in values) {
401
+
402
+ const value = values[name];
403
+ if (Array.isArray(value)) {
404
+
405
+ didChange = this.setJointValue(name, ...value) || didChange;
406
+
407
+ } else {
408
+
409
+ didChange = this.setJointValue(name, value) || didChange;
410
+
411
+ }
412
+
413
+ }
414
+
415
+ return didChange;
416
+
417
+ }
418
+
419
+ }
420
+
421
+ /*
422
+ Reference coordinate frames for THREE.js and ROS.
423
+ Both coordinate systems are right handed so the URDF is instantiated without
424
+ frame transforms. The resulting model can be rotated to rectify the proper up,
425
+ right, and forward directions
426
+
427
+ THREE.js
428
+ Y
429
+ |
430
+ |
431
+ .-----X
432
+
433
+ Z
434
+
435
+ ROS URDf
436
+ Z
437
+ | X
438
+ | /
439
+ Y-----.
440
+
441
+ */
442
+
443
+ const tempQuaternion = new THREE__namespace.Quaternion();
444
+ const tempEuler = new THREE__namespace.Euler();
445
+
446
+ // take a vector "x y z" and process it into
447
+ // an array [x, y, z]
448
+ function processTuple(val) {
449
+
450
+ if (!val) return [0, 0, 0];
451
+ return val.trim().split(/\s+/g).map(num => parseFloat(num));
452
+
453
+ }
454
+
455
+ // applies a rotation a threejs object in URDF order
456
+ function applyRotation(obj, rpy, additive = false) {
457
+
458
+ // if additive is true the rotation is applied in
459
+ // addition to the existing rotation
460
+ if (!additive) obj.rotation.set(0, 0, 0);
461
+
462
+ tempEuler.set(rpy[0], rpy[1], rpy[2], 'ZYX');
463
+ tempQuaternion.setFromEuler(tempEuler);
464
+ tempQuaternion.multiply(obj.quaternion);
465
+ obj.quaternion.copy(tempQuaternion);
466
+
426
467
  }
427
468
 
428
- /*
429
- Reference coordinate frames for THREE.js and ROS.
430
- Both coordinate systems are right handed so the URDF is instantiated without
431
- frame transforms. The resulting model can be rotated to rectify the proper up,
432
- right, and forward directions
433
-
434
- THREE.js
435
- Y
436
- |
437
- |
438
- .-----X
439
-
440
- Z
441
-
442
- ROS URDf
443
- Z
444
- | X
445
- |
446
- Y-----.
447
-
448
- */
449
-
450
- const tempQuaternion = new THREE__namespace.Quaternion();
451
- const tempEuler = new THREE__namespace.Euler();
452
-
453
- // take a vector "x y z" and process it into
454
- // an array [x, y, z]
455
- function processTuple(val) {
456
-
457
- if (!val) return [0, 0, 0];
458
- return val.trim().split(/\s+/g).map(num => parseFloat(num));
459
-
460
- }
461
-
462
- // applies a rotation a threejs object in URDF order
463
- function applyRotation(obj, rpy, additive = false) {
464
-
465
- // if additive is true the rotation is applied in
466
- // addition to the existing rotation
467
- if (!additive) obj.rotation.set(0, 0, 0);
468
-
469
- tempEuler.set(rpy[0], rpy[1], rpy[2], 'ZYX');
470
- tempQuaternion.setFromEuler(tempEuler);
471
- tempQuaternion.multiply(obj.quaternion);
472
- obj.quaternion.copy(tempQuaternion);
473
-
474
- }
475
-
476
- /* URDFLoader Class */
477
- // Loads and reads a URDF file into a THREEjs Object3D format
478
- class URDFLoader {
479
-
480
- constructor(manager) {
481
-
482
- this.manager = manager || THREE__namespace.DefaultLoadingManager;
483
- this.loadMeshCb = this.defaultMeshLoader.bind(this);
484
- this.parseVisual = true;
485
- this.parseCollision = false;
486
- this.packages = '';
487
- this.workingPath = '';
488
- this.fetchOptions = {};
489
-
490
- }
491
-
492
- /* Public API */
493
- loadAsync(urdf) {
494
-
495
- return new Promise((resolve, reject) => {
496
-
497
- this.load(urdf, resolve, null, reject);
498
-
499
- });
500
-
501
- }
502
-
503
- // urdf: The path to the URDF within the package OR absolute
504
- // onComplete: Callback that is passed the model once loaded
505
- load(urdf, onComplete, onProgress, onError) {
506
-
507
- // Check if a full URI is specified before
508
- // prepending the package info
509
- const manager = this.manager;
510
- const workingPath = THREE__namespace.LoaderUtils.extractUrlBase(urdf);
511
- const urdfPath = this.manager.resolveURL(urdf);
512
-
513
- manager.itemStart(urdfPath);
514
-
515
- fetch(urdfPath, this.fetchOptions)
516
- .then(res => {
517
-
518
- if (res.ok) {
519
-
520
- if (onProgress) {
521
-
522
- onProgress(null);
523
-
524
- }
525
- return res.text();
526
-
527
- } else {
528
-
529
- throw new Error(`URDFLoader: Failed to load url '${ urdfPath }' with error code ${ res.status } : ${ res.statusText }.`);
530
-
531
- }
532
-
533
- })
534
- .then(data => {
535
-
536
- if (this.workingPath === '') {
537
-
538
- this.workingPath = workingPath;
539
-
540
- }
541
-
542
- const model = this.parse(data);
543
- onComplete(model);
544
- manager.itemEnd(urdfPath);
545
-
546
- })
547
- .catch(e => {
548
-
549
- if (onError) {
550
-
551
- onError(e);
552
-
553
- } else {
554
-
555
- console.error('URDFLoader: Error loading file.', e);
556
-
557
- }
558
- manager.itemError(urdfPath);
559
- manager.itemEnd(urdfPath);
560
-
561
- });
562
-
563
- }
564
-
565
- parse(content) {
566
-
567
- const packages = this.packages;
568
- const loadMeshCb = this.loadMeshCb;
569
- const parseVisual = this.parseVisual;
570
- const parseCollision = this.parseCollision;
571
- const workingPath = this.workingPath;
572
- const manager = this.manager;
573
- const linkMap = {};
574
- const jointMap = {};
575
- const materialMap = {};
576
-
577
- // Resolves the path of mesh files
578
- function resolvePath(path) {
579
-
580
- if (!/^package:\/\//.test(path)) {
581
-
582
- return workingPath ? workingPath + path : path;
583
-
584
- }
585
-
586
- // Remove "package://" keyword and split meshPath at the first slash
587
- const [targetPkg, relPath] = path.replace(/^package:\/\//, '').split(/\/(.+)/);
588
-
589
- if (typeof packages === 'string') {
590
-
591
- // "pkg" is one single package
592
- if (packages.endsWith(targetPkg)) {
593
-
594
- // "pkg" is the target package
595
- return packages + '/' + relPath;
596
-
597
- } else {
598
-
599
- // Assume "pkg" is the target package's parent directory
600
- return packages + '/' + targetPkg + '/' + relPath;
601
-
602
- }
603
-
604
- } else if (packages instanceof Function) {
605
-
606
- return packages(targetPkg) + '/' + relPath;
607
-
608
- } else if (typeof packages === 'object') {
609
-
610
- // "pkg" is a map of packages
611
- if (targetPkg in packages) {
612
-
613
- return packages[targetPkg] + '/' + relPath;
614
-
615
- } else {
616
-
617
- console.error(`URDFLoader : ${ targetPkg } not found in provided package list.`);
618
- return null;
619
-
620
- }
621
-
622
- }
623
-
624
- }
625
-
626
- // Process the URDF text format
627
- function processUrdf(data) {
628
-
629
- let children;
630
- if (data instanceof Document) {
631
-
632
- children = [ ...data.children ];
633
-
634
- } else if (data instanceof Element) {
635
-
636
- children = [ data ];
637
-
638
- } else {
639
-
640
- const parser = new DOMParser();
641
- const urdf = parser.parseFromString(data, 'text/xml');
642
- children = [ ...urdf.children ];
643
-
644
- }
645
-
646
- const robotNode = children.filter(c => c.nodeName === 'robot').pop();
647
- return processRobot(robotNode);
648
-
649
- }
650
-
651
- // Process the <robot> node
652
- function processRobot(robot) {
653
-
654
- const robotNodes = [ ...robot.children ];
655
- const links = robotNodes.filter(c => c.nodeName.toLowerCase() === 'link');
656
- const joints = robotNodes.filter(c => c.nodeName.toLowerCase() === 'joint');
657
- const materials = robotNodes.filter(c => c.nodeName.toLowerCase() === 'material');
658
- const obj = new URDFRobot();
659
-
660
- obj.robotName = robot.getAttribute('name');
661
- obj.urdfRobotNode = robot;
662
-
663
- // Create the <material> map
664
- materials.forEach(m => {
665
-
666
- const name = m.getAttribute('name');
667
- materialMap[name] = processMaterial(m);
668
-
669
- });
670
-
671
- // Create the <link> map
672
- const visualMap = {};
673
- const colliderMap = {};
674
- links.forEach(l => {
675
-
676
- const name = l.getAttribute('name');
677
- const isRoot = robot.querySelector(`child[link="${ name }"]`) === null;
678
- linkMap[name] = processLink(l, visualMap, colliderMap, isRoot ? obj : null);
679
-
680
- });
681
-
682
- // Create the <joint> map
683
- joints.forEach(j => {
684
-
685
- const name = j.getAttribute('name');
686
- jointMap[name] = processJoint(j);
687
-
688
- });
689
-
690
- obj.joints = jointMap;
691
- obj.links = linkMap;
692
- obj.colliders = colliderMap;
693
- obj.visual = visualMap;
694
-
695
- // Link up mimic joints
696
- const jointList = Object.values(jointMap);
697
- jointList.forEach(j => {
698
-
699
- if (j instanceof URDFMimicJoint) {
700
-
701
- jointMap[j.mimicJoint].mimicJoints.push(j);
702
-
703
- }
704
-
705
- });
706
-
707
- // Detect infinite loops of mimic joints
708
- jointList.forEach(j => {
709
-
710
- const uniqueJoints = new Set();
711
- const iterFunction = joint => {
712
-
713
- if (uniqueJoints.has(joint)) {
714
-
715
- throw new Error('URDFLoader: Detected an infinite loop of mimic joints.');
716
-
717
- }
718
-
719
- uniqueJoints.add(joint);
720
- joint.mimicJoints.forEach(j => {
721
-
722
- iterFunction(j);
723
-
724
- });
725
-
726
- };
727
-
728
- iterFunction(j);
729
- });
730
-
731
- obj.frames = {
732
- ...colliderMap,
733
- ...visualMap,
734
- ...linkMap,
735
- ...jointMap,
736
- };
737
-
738
- return obj;
739
-
740
- }
741
-
742
- // Process joint nodes and parent them
743
- function processJoint(joint) {
744
-
745
- const children = [ ...joint.children ];
746
- const jointType = joint.getAttribute('type');
747
-
748
- let obj;
749
-
750
- const mimicTag = children.find(n => n.nodeName.toLowerCase() === 'mimic');
751
- if (mimicTag) {
752
-
753
- obj = new URDFMimicJoint();
754
- obj.mimicJoint = mimicTag.getAttribute('joint');
755
- obj.multiplier = parseFloat(mimicTag.getAttribute('multiplier') || 1.0);
756
- obj.offset = parseFloat(mimicTag.getAttribute('offset') || 0.0);
757
-
758
- } else {
759
-
760
- obj = new URDFJoint();
761
-
762
- }
763
-
764
- obj.urdfNode = joint;
765
- obj.name = joint.getAttribute('name');
766
- obj.urdfName = obj.name;
767
- obj.jointType = jointType;
768
-
769
- let parent = null;
770
- let child = null;
771
- let xyz = [0, 0, 0];
772
- let rpy = [0, 0, 0];
773
-
774
- // Extract the attributes
775
- children.forEach(n => {
776
-
777
- const type = n.nodeName.toLowerCase();
778
- if (type === 'origin') {
779
-
780
- xyz = processTuple(n.getAttribute('xyz'));
781
- rpy = processTuple(n.getAttribute('rpy'));
782
-
783
- } else if (type === 'child') {
784
-
785
- child = linkMap[n.getAttribute('link')];
786
-
787
- } else if (type === 'parent') {
788
-
789
- parent = linkMap[n.getAttribute('link')];
790
-
791
- } else if (type === 'limit') {
792
-
793
- obj.limit.lower = parseFloat(n.getAttribute('lower') || obj.limit.lower);
794
- obj.limit.upper = parseFloat(n.getAttribute('upper') || obj.limit.upper);
795
-
796
- }
797
- });
798
-
799
- // Join the links
800
- parent.add(obj);
801
- obj.add(child);
802
- applyRotation(obj, rpy);
803
- obj.position.set(xyz[0], xyz[1], xyz[2]);
804
-
805
- // Set up the rotate function
806
- const axisNode = children.filter(n => n.nodeName.toLowerCase() === 'axis')[0];
807
-
808
- if (axisNode) {
809
-
810
- const axisXYZ = axisNode.getAttribute('xyz').split(/\s+/g).map(num => parseFloat(num));
811
- obj.axis = new THREE__namespace.Vector3(axisXYZ[0], axisXYZ[1], axisXYZ[2]);
812
- obj.axis.normalize();
813
-
814
- }
815
-
816
- return obj;
817
-
818
- }
819
-
820
- // Process the <link> nodes
821
- function processLink(link, visualMap, colliderMap, target = null) {
822
-
823
- if (target === null) {
824
-
825
- target = new URDFLink();
826
-
827
- }
828
-
829
- const children = [ ...link.children ];
830
- target.name = link.getAttribute('name');
831
- target.urdfName = target.name;
832
- target.urdfNode = link;
833
-
834
- if (parseVisual) {
835
-
836
- const visualNodes = children.filter(n => n.nodeName.toLowerCase() === 'visual');
837
- visualNodes.forEach(vn => {
838
-
839
- const v = processLinkElement(vn, materialMap);
840
- target.add(v);
841
-
842
- if (vn.hasAttribute('name')) {
843
-
844
- const name = vn.getAttribute('name');
845
- v.name = name;
846
- v.urdfName = name;
847
- visualMap[name] = v;
848
-
849
- }
850
-
851
- });
852
-
853
- }
854
-
855
- if (parseCollision) {
856
-
857
- const collisionNodes = children.filter(n => n.nodeName.toLowerCase() === 'collision');
858
- collisionNodes.forEach(cn => {
859
-
860
- const c = processLinkElement(cn);
861
- target.add(c);
862
-
863
- if (cn.hasAttribute('name')) {
864
-
865
- const name = cn.getAttribute('name');
866
- c.name = name;
867
- c.urdfName = name;
868
- colliderMap[name] = c;
869
-
870
- }
871
-
872
- });
873
-
874
- }
875
-
876
- return target;
877
-
878
- }
879
-
880
- function processMaterial(node) {
881
-
882
- const matNodes = [ ...node.children ];
883
- const material = new THREE__namespace.MeshPhongMaterial();
884
-
885
- material.name = node.getAttribute('name') || '';
886
- matNodes.forEach(n => {
887
-
888
- const type = n.nodeName.toLowerCase();
889
- if (type === 'color') {
890
-
891
- const rgba =
892
- n
893
- .getAttribute('rgba')
894
- .split(/\s/g)
895
- .map(v => parseFloat(v));
896
-
897
- material.color.setRGB(rgba[0], rgba[1], rgba[2]).convertSRGBToLinear();
898
- material.opacity = rgba[3];
899
- material.transparent = rgba[3] < 1;
900
- material.depthWrite = !material.transparent;
901
-
902
- } else if (type === 'texture') {
903
-
904
- // The URDF spec does not require that the <texture/> tag include
905
- // a filename attribute so skip loading the texture if not provided.
906
- const filename = n.getAttribute('filename');
907
- if (filename) {
908
-
909
- const loader = new THREE__namespace.TextureLoader(manager);
910
- const filePath = resolvePath(filename);
911
- material.map = loader.load(filePath);
912
- material.map.encoding = THREE__namespace.sRGBEncoding;
913
-
914
- }
915
-
916
- }
917
- });
918
-
919
- return material;
920
-
921
- }
922
-
923
- // Process the visual and collision nodes into meshes
924
- function processLinkElement(vn, materialMap = {}) {
925
-
926
- const isCollisionNode = vn.nodeName.toLowerCase() === 'collision';
927
- const children = [ ...vn.children ];
928
- let material = null;
929
-
930
- // get the material first
931
- const materialNode = children.filter(n => n.nodeName.toLowerCase() === 'material')[0];
932
- if (materialNode) {
933
-
934
- const name = materialNode.getAttribute('name');
935
- if (name && name in materialMap) {
936
-
937
- material = materialMap[name];
938
-
939
- } else {
940
-
941
- material = processMaterial(materialNode);
942
-
943
- }
944
-
945
- } else {
946
-
947
- material = new THREE__namespace.MeshPhongMaterial();
948
-
949
- }
950
-
951
- const group = isCollisionNode ? new URDFCollider() : new URDFVisual();
952
- group.urdfNode = vn;
953
-
954
- children.forEach(n => {
955
-
956
- const type = n.nodeName.toLowerCase();
957
- if (type === 'geometry') {
958
-
959
- const geoType = n.children[0].nodeName.toLowerCase();
960
- if (geoType === 'mesh') {
961
-
962
- const filename = n.children[0].getAttribute('filename');
963
- const filePath = resolvePath(filename);
964
-
965
- // file path is null if a package directory is not provided.
966
- if (filePath !== null) {
967
-
968
- const scaleAttr = n.children[0].getAttribute('scale');
969
- if (scaleAttr) {
970
-
971
- const scale = processTuple(scaleAttr);
972
- group.scale.set(scale[0], scale[1], scale[2]);
973
-
974
- }
975
-
976
- loadMeshCb(filePath, manager, (obj, err) => {
977
-
978
- if (err) {
979
-
980
- console.error('URDFLoader: Error loading mesh.', err);
981
-
982
- } else if (obj) {
983
-
984
- if (obj instanceof THREE__namespace.Mesh) {
985
-
986
- obj.material = material;
987
-
988
- }
989
-
990
- // We don't expect non identity rotations or positions. In the case of
991
- // COLLADA files the model might come in with a custom scale for unit
992
- // conversion.
993
- obj.position.set(0, 0, 0);
994
- obj.quaternion.identity();
995
- group.add(obj);
996
-
997
- }
998
-
999
- });
1000
-
1001
- }
1002
-
1003
- } else if (geoType === 'box') {
1004
-
1005
- const primitiveModel = new THREE__namespace.Mesh();
1006
- primitiveModel.geometry = new THREE__namespace.BoxBufferGeometry(1, 1, 1);
1007
- primitiveModel.material = material;
1008
-
1009
- const size = processTuple(n.children[0].getAttribute('size'));
1010
- primitiveModel.scale.set(size[0], size[1], size[2]);
1011
-
1012
- group.add(primitiveModel);
1013
-
1014
- } else if (geoType === 'sphere') {
1015
-
1016
- const primitiveModel = new THREE__namespace.Mesh();
1017
- primitiveModel.geometry = new THREE__namespace.SphereBufferGeometry(1, 30, 30);
1018
- primitiveModel.material = material;
1019
-
1020
- const radius = parseFloat(n.children[0].getAttribute('radius')) || 0;
1021
- primitiveModel.scale.set(radius, radius, radius);
1022
-
1023
- group.add(primitiveModel);
1024
-
1025
- } else if (geoType === 'cylinder') {
1026
-
1027
- const primitiveModel = new THREE__namespace.Mesh();
1028
- primitiveModel.geometry = new THREE__namespace.CylinderBufferGeometry(1, 1, 1, 30);
1029
- primitiveModel.material = material;
1030
-
1031
- const radius = parseFloat(n.children[0].getAttribute('radius')) || 0;
1032
- const length = parseFloat(n.children[0].getAttribute('length')) || 0;
1033
- primitiveModel.scale.set(radius, length, radius);
1034
- primitiveModel.rotation.set(Math.PI / 2, 0, 0);
1035
-
1036
- group.add(primitiveModel);
1037
-
1038
- }
1039
-
1040
- } else if (type === 'origin') {
1041
-
1042
- const xyz = processTuple(n.getAttribute('xyz'));
1043
- const rpy = processTuple(n.getAttribute('rpy'));
1044
-
1045
- group.position.set(xyz[0], xyz[1], xyz[2]);
1046
- group.rotation.set(0, 0, 0);
1047
- applyRotation(group, rpy);
1048
-
1049
- }
1050
-
1051
- });
1052
-
1053
- return group;
1054
-
1055
- }
1056
-
1057
- return processUrdf(content);
1058
-
1059
- }
1060
-
1061
- // Default mesh loading function
1062
- defaultMeshLoader(path, manager, done) {
1063
-
1064
- if (/\.stl$/i.test(path)) {
1065
-
1066
- const loader = new STLLoader_js.STLLoader(manager);
1067
- loader.load(path, geom => {
1068
- const mesh = new THREE__namespace.Mesh(geom, new THREE__namespace.MeshPhongMaterial());
1069
- done(mesh);
1070
- });
1071
-
1072
- } else if (/\.dae$/i.test(path)) {
1073
-
1074
- const loader = new ColladaLoader_js.ColladaLoader(manager);
1075
- loader.load(path, dae => done(dae.scene));
1076
-
1077
- } else {
1078
-
1079
- console.warn(`URDFLoader: Could not load model at ${ path }.\nNo loader available`);
1080
-
1081
- }
1082
-
1083
- }
1084
-
469
+ /* URDFLoader Class */
470
+ // Loads and reads a URDF file into a THREEjs Object3D format
471
+ class URDFLoader {
472
+
473
+ constructor(manager) {
474
+
475
+ this.manager = manager || THREE__namespace.DefaultLoadingManager;
476
+ this.loadMeshCb = this.defaultMeshLoader.bind(this);
477
+ this.parseVisual = true;
478
+ this.parseCollision = false;
479
+ this.packages = '';
480
+ this.workingPath = '';
481
+ this.fetchOptions = {};
482
+
483
+ }
484
+
485
+ /* Public API */
486
+ loadAsync(urdf) {
487
+
488
+ return new Promise((resolve, reject) => {
489
+
490
+ this.load(urdf, resolve, null, reject);
491
+
492
+ });
493
+
494
+ }
495
+
496
+ // urdf: The path to the URDF within the package OR absolute
497
+ // onComplete: Callback that is passed the model once loaded
498
+ load(urdf, onComplete, onProgress, onError) {
499
+
500
+ // Check if a full URI is specified before
501
+ // prepending the package info
502
+ const manager = this.manager;
503
+ const workingPath = THREE__namespace.LoaderUtils.extractUrlBase(urdf);
504
+ const urdfPath = this.manager.resolveURL(urdf);
505
+
506
+ manager.itemStart(urdfPath);
507
+
508
+ fetch(urdfPath, this.fetchOptions)
509
+ .then(res => {
510
+
511
+ if (res.ok) {
512
+
513
+ if (onProgress) {
514
+
515
+ onProgress(null);
516
+
517
+ }
518
+ return res.text();
519
+
520
+ } else {
521
+
522
+ throw new Error(`URDFLoader: Failed to load url '${ urdfPath }' with error code ${ res.status } : ${ res.statusText }.`);
523
+
524
+ }
525
+
526
+ })
527
+ .then(data => {
528
+
529
+ if (this.workingPath === '') {
530
+
531
+ this.workingPath = workingPath;
532
+
533
+ }
534
+
535
+ const model = this.parse(data);
536
+ onComplete(model);
537
+ manager.itemEnd(urdfPath);
538
+
539
+ })
540
+ .catch(e => {
541
+
542
+ if (onError) {
543
+
544
+ onError(e);
545
+
546
+ } else {
547
+
548
+ console.error('URDFLoader: Error loading file.', e);
549
+
550
+ }
551
+ manager.itemError(urdfPath);
552
+ manager.itemEnd(urdfPath);
553
+
554
+ });
555
+
556
+ }
557
+
558
+ parse(content) {
559
+
560
+ const packages = this.packages;
561
+ const loadMeshCb = this.loadMeshCb;
562
+ const parseVisual = this.parseVisual;
563
+ const parseCollision = this.parseCollision;
564
+ const workingPath = this.workingPath;
565
+ const manager = this.manager;
566
+ const linkMap = {};
567
+ const jointMap = {};
568
+ const materialMap = {};
569
+
570
+ // Resolves the path of mesh files
571
+ function resolvePath(path) {
572
+
573
+ if (!/^package:\/\//.test(path)) {
574
+
575
+ return workingPath ? workingPath + path : path;
576
+
577
+ }
578
+
579
+ // Remove "package://" keyword and split meshPath at the first slash
580
+ const [targetPkg, relPath] = path.replace(/^package:\/\//, '').split(/\/(.+)/);
581
+
582
+ if (typeof packages === 'string') {
583
+
584
+ // "pkg" is one single package
585
+ if (packages.endsWith(targetPkg)) {
586
+
587
+ // "pkg" is the target package
588
+ return packages + '/' + relPath;
589
+
590
+ } else {
591
+
592
+ // Assume "pkg" is the target package's parent directory
593
+ return packages + '/' + targetPkg + '/' + relPath;
594
+
595
+ }
596
+
597
+ } else if (packages instanceof Function) {
598
+
599
+ return packages(targetPkg) + '/' + relPath;
600
+
601
+ } else if (typeof packages === 'object') {
602
+
603
+ // "pkg" is a map of packages
604
+ if (targetPkg in packages) {
605
+
606
+ return packages[targetPkg] + '/' + relPath;
607
+
608
+ } else {
609
+
610
+ console.error(`URDFLoader : ${ targetPkg } not found in provided package list.`);
611
+ return null;
612
+
613
+ }
614
+
615
+ }
616
+
617
+ }
618
+
619
+ // Process the URDF text format
620
+ function processUrdf(data) {
621
+
622
+ let children;
623
+ if (data instanceof Document) {
624
+
625
+ children = [ ...data.children ];
626
+
627
+ } else if (data instanceof Element) {
628
+
629
+ children = [ data ];
630
+
631
+ } else {
632
+
633
+ const parser = new DOMParser();
634
+ const urdf = parser.parseFromString(data, 'text/xml');
635
+ children = [ ...urdf.children ];
636
+
637
+ }
638
+
639
+ const robotNode = children.filter(c => c.nodeName === 'robot').pop();
640
+ return processRobot(robotNode);
641
+
642
+ }
643
+
644
+ // Process the <robot> node
645
+ function processRobot(robot) {
646
+
647
+ const robotNodes = [ ...robot.children ];
648
+ const links = robotNodes.filter(c => c.nodeName.toLowerCase() === 'link');
649
+ const joints = robotNodes.filter(c => c.nodeName.toLowerCase() === 'joint');
650
+ const materials = robotNodes.filter(c => c.nodeName.toLowerCase() === 'material');
651
+ const obj = new URDFRobot();
652
+
653
+ obj.robotName = robot.getAttribute('name');
654
+ obj.urdfRobotNode = robot;
655
+
656
+ // Create the <material> map
657
+ materials.forEach(m => {
658
+
659
+ const name = m.getAttribute('name');
660
+ materialMap[name] = processMaterial(m);
661
+
662
+ });
663
+
664
+ // Create the <link> map
665
+ const visualMap = {};
666
+ const colliderMap = {};
667
+ links.forEach(l => {
668
+
669
+ const name = l.getAttribute('name');
670
+ const isRoot = robot.querySelector(`child[link="${ name }"]`) === null;
671
+ linkMap[name] = processLink(l, visualMap, colliderMap, isRoot ? obj : null);
672
+
673
+ });
674
+
675
+ // Create the <joint> map
676
+ joints.forEach(j => {
677
+
678
+ const name = j.getAttribute('name');
679
+ jointMap[name] = processJoint(j);
680
+
681
+ });
682
+
683
+ obj.joints = jointMap;
684
+ obj.links = linkMap;
685
+ obj.colliders = colliderMap;
686
+ obj.visual = visualMap;
687
+
688
+ // Link up mimic joints
689
+ const jointList = Object.values(jointMap);
690
+ jointList.forEach(j => {
691
+
692
+ if (j instanceof URDFMimicJoint) {
693
+
694
+ jointMap[j.mimicJoint].mimicJoints.push(j);
695
+
696
+ }
697
+
698
+ });
699
+
700
+ // Detect infinite loops of mimic joints
701
+ jointList.forEach(j => {
702
+
703
+ const uniqueJoints = new Set();
704
+ const iterFunction = joint => {
705
+
706
+ if (uniqueJoints.has(joint)) {
707
+
708
+ throw new Error('URDFLoader: Detected an infinite loop of mimic joints.');
709
+
710
+ }
711
+
712
+ uniqueJoints.add(joint);
713
+ joint.mimicJoints.forEach(j => {
714
+
715
+ iterFunction(j);
716
+
717
+ });
718
+
719
+ };
720
+
721
+ iterFunction(j);
722
+ });
723
+
724
+ obj.frames = {
725
+ ...colliderMap,
726
+ ...visualMap,
727
+ ...linkMap,
728
+ ...jointMap,
729
+ };
730
+
731
+ return obj;
732
+
733
+ }
734
+
735
+ // Process joint nodes and parent them
736
+ function processJoint(joint) {
737
+
738
+ const children = [ ...joint.children ];
739
+ const jointType = joint.getAttribute('type');
740
+
741
+ let obj;
742
+
743
+ const mimicTag = children.find(n => n.nodeName.toLowerCase() === 'mimic');
744
+ if (mimicTag) {
745
+
746
+ obj = new URDFMimicJoint();
747
+ obj.mimicJoint = mimicTag.getAttribute('joint');
748
+ obj.multiplier = parseFloat(mimicTag.getAttribute('multiplier') || 1.0);
749
+ obj.offset = parseFloat(mimicTag.getAttribute('offset') || 0.0);
750
+
751
+ } else {
752
+
753
+ obj = new URDFJoint();
754
+
755
+ }
756
+
757
+ obj.urdfNode = joint;
758
+ obj.name = joint.getAttribute('name');
759
+ obj.urdfName = obj.name;
760
+ obj.jointType = jointType;
761
+
762
+ let parent = null;
763
+ let child = null;
764
+ let xyz = [0, 0, 0];
765
+ let rpy = [0, 0, 0];
766
+
767
+ // Extract the attributes
768
+ children.forEach(n => {
769
+
770
+ const type = n.nodeName.toLowerCase();
771
+ if (type === 'origin') {
772
+
773
+ xyz = processTuple(n.getAttribute('xyz'));
774
+ rpy = processTuple(n.getAttribute('rpy'));
775
+
776
+ } else if (type === 'child') {
777
+
778
+ child = linkMap[n.getAttribute('link')];
779
+
780
+ } else if (type === 'parent') {
781
+
782
+ parent = linkMap[n.getAttribute('link')];
783
+
784
+ } else if (type === 'limit') {
785
+
786
+ obj.limit.lower = parseFloat(n.getAttribute('lower') || obj.limit.lower);
787
+ obj.limit.upper = parseFloat(n.getAttribute('upper') || obj.limit.upper);
788
+
789
+ }
790
+ });
791
+
792
+ // Join the links
793
+ parent.add(obj);
794
+ obj.add(child);
795
+ applyRotation(obj, rpy);
796
+ obj.position.set(xyz[0], xyz[1], xyz[2]);
797
+
798
+ // Set up the rotate function
799
+ const axisNode = children.filter(n => n.nodeName.toLowerCase() === 'axis')[0];
800
+
801
+ if (axisNode) {
802
+
803
+ const axisXYZ = axisNode.getAttribute('xyz').split(/\s+/g).map(num => parseFloat(num));
804
+ obj.axis = new THREE__namespace.Vector3(axisXYZ[0], axisXYZ[1], axisXYZ[2]);
805
+ obj.axis.normalize();
806
+
807
+ }
808
+
809
+ return obj;
810
+
811
+ }
812
+
813
+ // Process the <link> nodes
814
+ function processLink(link, visualMap, colliderMap, target = null) {
815
+
816
+ if (target === null) {
817
+
818
+ target = new URDFLink();
819
+
820
+ }
821
+
822
+ const children = [ ...link.children ];
823
+ target.name = link.getAttribute('name');
824
+ target.urdfName = target.name;
825
+ target.urdfNode = link;
826
+
827
+ if (parseVisual) {
828
+
829
+ const visualNodes = children.filter(n => n.nodeName.toLowerCase() === 'visual');
830
+ visualNodes.forEach(vn => {
831
+
832
+ const v = processLinkElement(vn, materialMap);
833
+ target.add(v);
834
+
835
+ if (vn.hasAttribute('name')) {
836
+
837
+ const name = vn.getAttribute('name');
838
+ v.name = name;
839
+ v.urdfName = name;
840
+ visualMap[name] = v;
841
+
842
+ }
843
+
844
+ });
845
+
846
+ }
847
+
848
+ if (parseCollision) {
849
+
850
+ const collisionNodes = children.filter(n => n.nodeName.toLowerCase() === 'collision');
851
+ collisionNodes.forEach(cn => {
852
+
853
+ const c = processLinkElement(cn);
854
+ target.add(c);
855
+
856
+ if (cn.hasAttribute('name')) {
857
+
858
+ const name = cn.getAttribute('name');
859
+ c.name = name;
860
+ c.urdfName = name;
861
+ colliderMap[name] = c;
862
+
863
+ }
864
+
865
+ });
866
+
867
+ }
868
+
869
+ return target;
870
+
871
+ }
872
+
873
+ function processMaterial(node) {
874
+
875
+ const matNodes = [ ...node.children ];
876
+ const material = new THREE__namespace.MeshPhongMaterial();
877
+
878
+ material.name = node.getAttribute('name') || '';
879
+ matNodes.forEach(n => {
880
+
881
+ const type = n.nodeName.toLowerCase();
882
+ if (type === 'color') {
883
+
884
+ const rgba =
885
+ n
886
+ .getAttribute('rgba')
887
+ .split(/\s/g)
888
+ .map(v => parseFloat(v));
889
+
890
+ material.color.setRGB(rgba[0], rgba[1], rgba[2]).convertSRGBToLinear();
891
+ material.opacity = rgba[3];
892
+ material.transparent = rgba[3] < 1;
893
+ material.depthWrite = !material.transparent;
894
+
895
+ } else if (type === 'texture') {
896
+
897
+ // The URDF spec does not require that the <texture/> tag include
898
+ // a filename attribute so skip loading the texture if not provided.
899
+ const filename = n.getAttribute('filename');
900
+ if (filename) {
901
+
902
+ const loader = new THREE__namespace.TextureLoader(manager);
903
+ const filePath = resolvePath(filename);
904
+ material.map = loader.load(filePath);
905
+ material.map.encoding = THREE__namespace.sRGBEncoding;
906
+
907
+ }
908
+
909
+ }
910
+ });
911
+
912
+ return material;
913
+
914
+ }
915
+
916
+ // Process the visual and collision nodes into meshes
917
+ function processLinkElement(vn, materialMap = {}) {
918
+
919
+ const isCollisionNode = vn.nodeName.toLowerCase() === 'collision';
920
+ const children = [ ...vn.children ];
921
+ let material = null;
922
+
923
+ // get the material first
924
+ const materialNode = children.filter(n => n.nodeName.toLowerCase() === 'material')[0];
925
+ if (materialNode) {
926
+
927
+ const name = materialNode.getAttribute('name');
928
+ if (name && name in materialMap) {
929
+
930
+ material = materialMap[name];
931
+
932
+ } else {
933
+
934
+ material = processMaterial(materialNode);
935
+
936
+ }
937
+
938
+ } else {
939
+
940
+ material = new THREE__namespace.MeshPhongMaterial();
941
+
942
+ }
943
+
944
+ const group = isCollisionNode ? new URDFCollider() : new URDFVisual();
945
+ group.urdfNode = vn;
946
+
947
+ children.forEach(n => {
948
+
949
+ const type = n.nodeName.toLowerCase();
950
+ if (type === 'geometry') {
951
+
952
+ const geoType = n.children[0].nodeName.toLowerCase();
953
+ if (geoType === 'mesh') {
954
+
955
+ const filename = n.children[0].getAttribute('filename');
956
+ const filePath = resolvePath(filename);
957
+
958
+ // file path is null if a package directory is not provided.
959
+ if (filePath !== null) {
960
+
961
+ const scaleAttr = n.children[0].getAttribute('scale');
962
+ if (scaleAttr) {
963
+
964
+ const scale = processTuple(scaleAttr);
965
+ group.scale.set(scale[0], scale[1], scale[2]);
966
+
967
+ }
968
+
969
+ loadMeshCb(filePath, manager, (obj, err) => {
970
+
971
+ if (err) {
972
+
973
+ console.error('URDFLoader: Error loading mesh.', err);
974
+
975
+ } else if (obj) {
976
+
977
+ if (obj instanceof THREE__namespace.Mesh) {
978
+
979
+ obj.material = material;
980
+
981
+ }
982
+
983
+ // We don't expect non identity rotations or positions. In the case of
984
+ // COLLADA files the model might come in with a custom scale for unit
985
+ // conversion.
986
+ obj.position.set(0, 0, 0);
987
+ obj.quaternion.identity();
988
+ group.add(obj);
989
+
990
+ }
991
+
992
+ });
993
+
994
+ }
995
+
996
+ } else if (geoType === 'box') {
997
+
998
+ const primitiveModel = new THREE__namespace.Mesh();
999
+ primitiveModel.geometry = new THREE__namespace.BoxGeometry(1, 1, 1);
1000
+ primitiveModel.material = material;
1001
+
1002
+ const size = processTuple(n.children[0].getAttribute('size'));
1003
+ primitiveModel.scale.set(size[0], size[1], size[2]);
1004
+
1005
+ group.add(primitiveModel);
1006
+
1007
+ } else if (geoType === 'sphere') {
1008
+
1009
+ const primitiveModel = new THREE__namespace.Mesh();
1010
+ primitiveModel.geometry = new THREE__namespace.SphereGeometry(1, 30, 30);
1011
+ primitiveModel.material = material;
1012
+
1013
+ const radius = parseFloat(n.children[0].getAttribute('radius')) || 0;
1014
+ primitiveModel.scale.set(radius, radius, radius);
1015
+
1016
+ group.add(primitiveModel);
1017
+
1018
+ } else if (geoType === 'cylinder') {
1019
+
1020
+ const primitiveModel = new THREE__namespace.Mesh();
1021
+ primitiveModel.geometry = new THREE__namespace.CylinderGeometry(1, 1, 1, 30);
1022
+ primitiveModel.material = material;
1023
+
1024
+ const radius = parseFloat(n.children[0].getAttribute('radius')) || 0;
1025
+ const length = parseFloat(n.children[0].getAttribute('length')) || 0;
1026
+ primitiveModel.scale.set(radius, length, radius);
1027
+ primitiveModel.rotation.set(Math.PI / 2, 0, 0);
1028
+
1029
+ group.add(primitiveModel);
1030
+
1031
+ }
1032
+
1033
+ } else if (type === 'origin') {
1034
+
1035
+ const xyz = processTuple(n.getAttribute('xyz'));
1036
+ const rpy = processTuple(n.getAttribute('rpy'));
1037
+
1038
+ group.position.set(xyz[0], xyz[1], xyz[2]);
1039
+ group.rotation.set(0, 0, 0);
1040
+ applyRotation(group, rpy);
1041
+
1042
+ }
1043
+
1044
+ });
1045
+
1046
+ return group;
1047
+
1048
+ }
1049
+
1050
+ return processUrdf(content);
1051
+
1052
+ }
1053
+
1054
+ // Default mesh loading function
1055
+ defaultMeshLoader(path, manager, done) {
1056
+
1057
+ if (/\.stl$/i.test(path)) {
1058
+
1059
+ const loader = new STLLoader_js.STLLoader(manager);
1060
+ loader.load(path, geom => {
1061
+ const mesh = new THREE__namespace.Mesh(geom, new THREE__namespace.MeshPhongMaterial());
1062
+ done(mesh);
1063
+ });
1064
+
1065
+ } else if (/\.dae$/i.test(path)) {
1066
+
1067
+ const loader = new ColladaLoader_js.ColladaLoader(manager);
1068
+ loader.load(path, dae => done(dae.scene));
1069
+
1070
+ } else {
1071
+
1072
+ console.warn(`URDFLoader: Could not load model at ${ path }.\nNo loader available`);
1073
+
1074
+ }
1075
+
1076
+ }
1077
+
1085
1078
  };
1086
1079
 
1087
1080
  return URDFLoader;