1 /*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23 /*
24
25 perform tests on all the joint types.
26 this should be done using the double precision version of the library.
27
28 usage:
29 test_joints [-nXXX] [-g] [-i] [-e] [path_to_textures]
30
31 if a test number is given then that specific test is performed, otherwise
32 all the tests are performed. the tests are numbered `xxyy', where xx
33 corresponds to the joint type and yy is the sub-test number. not every
34 number maps to an actual test.
35
36 flags:
37 i: the test is interactive.
38 g: turn off graphical display (can't use this with `i').
39 e: turn on occasional error perturbations
40 n: performe test XXX
41 some tests compute and display error values. these values are scaled so
42 <1 is good and >1 is bad. other tests just show graphical results which
43 you must verify visually.
44
45 */
46
47 #include <ctype.h>
48 #include <ode/ode.h>
49 #include <drawstuff/drawstuff.h>
50 #include "texturepath.h"
51
52 #ifdef _MSC_VER
53 #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
54 #endif
55
56 // select correct drawing functions
57 #ifdef dDOUBLE
58 #define dsDrawBox dsDrawBoxD
59 #endif
60
61
62 // some constants
63 #define NUM_JOINTS 10 // number of joints to test (the `xx' value)
64 #define SIDE (0.5f) // side length of a box - don't change this
65 #define MASS (1.0) // mass of a box
66 #define STEPSIZE 0.05
67
68
69 // dynamics objects
70 static dWorldID world;
71 static dBodyID body[2];
72 static dJointID joint;
73
74
75 // data from the command line arguments
76 static int cmd_test_num = -1;
77 static int cmd_interactive = 0;
78 static int cmd_graphics = 1;
79 static char *cmd_path_to_textures = NULL;
80 static int cmd_occasional_error = 0; // perturb occasionally
81
82
83 // info about the current test
84 struct TestInfo;
85 static int test_num = 0; // number of the current test
86 static int iteration = 0;
87 static int max_iterations = 0;
88 static dReal max_error = 0;
89
90 //****************************************************************************
91 // utility stuff
92
length(dVector3 a)93 static dReal length (dVector3 a)
94 {
95 return dSqrt (a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
96 }
97
98
99 // get the max difference between a 3x3 matrix and the identity
100
cmpIdentity(const dMatrix3 A)101 dReal cmpIdentity (const dMatrix3 A)
102 {
103 dMatrix3 I;
104 dSetZero (I,12);
105 I[0] = 1;
106 I[5] = 1;
107 I[10] = 1;
108 return dMaxDifference (A,I,3,3);
109 }
110
111 //****************************************************************************
112 // test world construction and utilities
113
constructWorldForTest(dReal gravity,int bodycount,dReal pos1x,dReal pos1y,dReal pos1z,dReal pos2x,dReal pos2y,dReal pos2z,dReal ax1x,dReal ax1y,dReal ax1z,dReal ax2x,dReal ax2y,dReal ax2z,dReal a1,dReal a2)114 void constructWorldForTest (dReal gravity, int bodycount,
115 /* body 1 pos */ dReal pos1x, dReal pos1y, dReal pos1z,
116 /* body 2 pos */ dReal pos2x, dReal pos2y, dReal pos2z,
117 /* body 1 rotation axis */ dReal ax1x, dReal ax1y, dReal ax1z,
118 /* body 1 rotation axis */ dReal ax2x, dReal ax2y, dReal ax2z,
119 /* rotation angles */ dReal a1, dReal a2)
120 {
121 // create world
122 world = dWorldCreate();
123 dWorldSetERP (world,0.2);
124 dWorldSetCFM (world,1e-6);
125 dWorldSetGravity (world,0,0,gravity);
126
127 dMass m;
128 dMassSetBox (&m,1,SIDE,SIDE,SIDE);
129 dMassAdjust (&m,MASS);
130
131 body[0] = dBodyCreate (world);
132 dBodySetMass (body[0],&m);
133 dBodySetPosition (body[0], pos1x, pos1y, pos1z);
134 dQuaternion q;
135 dQFromAxisAndAngle (q,ax1x,ax1y,ax1z,a1);
136 dBodySetQuaternion (body[0],q);
137
138 if (bodycount==2) {
139 body[1] = dBodyCreate (world);
140 dBodySetMass (body[1],&m);
141 dBodySetPosition (body[1], pos2x, pos2y, pos2z);
142 dQFromAxisAndAngle (q,ax2x,ax2y,ax2z,a2);
143 dBodySetQuaternion (body[1],q);
144 }
145 else body[1] = 0;
146 }
147
148
149 // add an oscillating torque to body 0
150
addOscillatingTorque(dReal tscale)151 void addOscillatingTorque (dReal tscale)
152 {
153 static dReal a=0;
154 dBodyAddTorque (body[0],tscale*cos(2*a),tscale*cos(2.7183*a),
155 tscale*cos(1.5708*a));
156 a += 0.01;
157 }
158
159
addOscillatingTorqueAbout(dReal tscale,dReal x,dReal y,dReal z)160 void addOscillatingTorqueAbout(dReal tscale, dReal x, dReal y, dReal z)
161 {
162 static dReal a=0;
163 dBodyAddTorque (body[0], tscale*cos(a) * x, tscale*cos(a) * y,
164 tscale * cos(a) * z);
165 a += 0.02;
166 }
167
168
169 // damp the rotational motion of body 0 a bit
170
dampRotationalMotion(dReal kd)171 void dampRotationalMotion (dReal kd)
172 {
173 const dReal *w = dBodyGetAngularVel (body[0]);
174 dBodyAddTorque (body[0],-kd*w[0],-kd*w[1],-kd*w[2]);
175 }
176
177
178 // add a spring force to keep the bodies together, otherwise they may fly
179 // apart with some joints.
180
addSpringForce(dReal ks)181 void addSpringForce (dReal ks)
182 {
183 const dReal *p1 = dBodyGetPosition (body[0]);
184 const dReal *p2 = dBodyGetPosition (body[1]);
185 dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),ks*(p2[2]-p1[2]));
186 dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),ks*(p1[2]-p2[2]));
187 }
188
189
190 // add an oscillating Force to body 0
191
addOscillatingForce(dReal fscale)192 void addOscillatingForce (dReal fscale)
193 {
194 static dReal a=0;
195 dBodyAddForce (body[0],fscale*cos(2*a),fscale*cos(2.7183*a),
196 fscale*cos(1.5708*a));
197 a += 0.01;
198 }
199
200 //****************************************************************************
201 // stuff specific to the tests
202 //
203 // 0xx : fixed
204 // 1xx : ball and socket
205 // 2xx : hinge
206 // 3xx : slider
207 // 4xx : hinge 2
208 // 5xx : contact
209 // 6xx : amotor
210 // 7xx : universal joint
211 // 8xx : PR joint (Prismatic and Rotoide)
212
213 // setup for the given test. return 0 if there is no such test
214
setupTest(int n)215 int setupTest (int n)
216 {
217 switch (n) {
218
219 // ********** fixed joint
220
221 case 0: { // 2 body
222 constructWorldForTest (0,2,
223 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
224 1,1,0, 1,1,0,
225 0.25*M_PI,0.25*M_PI);
226 joint = dJointCreateFixed (world,0);
227 dJointAttach (joint,body[0],body[1]);
228 dJointSetFixed (joint);
229 return 1;
230 }
231
232 case 1: { // 1 body to static env
233 constructWorldForTest (0,1,
234 0.5*SIDE,0.5*SIDE,1, 0,0,0,
235 1,0,0, 1,0,0,
236 0,0);
237 joint = dJointCreateFixed (world,0);
238 dJointAttach (joint,body[0],0);
239 dJointSetFixed (joint);
240 return 1;
241 }
242
243 case 2: { // 2 body with relative rotation
244 constructWorldForTest (0,2,
245 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
246 1,1,0, 1,1,0,
247 0.25*M_PI,-0.25*M_PI);
248 joint = dJointCreateFixed (world,0);
249 dJointAttach (joint,body[0],body[1]);
250 dJointSetFixed (joint);
251 return 1;
252 }
253
254 case 3: { // 1 body to static env with relative rotation
255 constructWorldForTest (0,1,
256 0.5*SIDE,0.5*SIDE,1, 0,0,0,
257 1,0,0, 1,0,0,
258 0.25*M_PI,0);
259 joint = dJointCreateFixed (world,0);
260 dJointAttach (joint,body[0],0);
261 dJointSetFixed (joint);
262 return 1;
263 }
264
265 // ********** hinge joint
266
267 case 200: // 2 body
268 constructWorldForTest (0,2,
269 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
270 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
271 joint = dJointCreateHinge (world,0);
272 dJointAttach (joint,body[0],body[1]);
273 dJointSetHingeAnchor (joint,0,0,1);
274 dJointSetHingeAxis (joint,1,-1,1.41421356);
275 return 1;
276
277 case 220: // hinge angle polarity test
278 case 221: // hinge angle rate test
279 constructWorldForTest (0,2,
280 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
281 1,0,0, 1,0,0, 0,0);
282 joint = dJointCreateHinge (world,0);
283 dJointAttach (joint,body[0],body[1]);
284 dJointSetHingeAnchor (joint,0,0,1);
285 dJointSetHingeAxis (joint,0,0,1);
286 max_iterations = 50;
287 return 1;
288
289 case 230: // hinge motor rate (and polarity) test
290 case 231: // ...with stops
291 constructWorldForTest (0,2,
292 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
293 1,0,0, 1,0,0, 0,0);
294 joint = dJointCreateHinge (world,0);
295 dJointAttach (joint,body[0],body[1]);
296 dJointSetHingeAnchor (joint,0,0,1);
297 dJointSetHingeAxis (joint,0,0,1);
298 dJointSetHingeParam (joint,dParamFMax,1);
299 if (n==231) {
300 dJointSetHingeParam (joint,dParamLoStop,-0.5);
301 dJointSetHingeParam (joint,dParamHiStop,0.5);
302 }
303 return 1;
304
305 case 250: // limit bounce test (gravity down)
306 case 251: { // ...gravity up
307 constructWorldForTest ((n==251) ? 0.1 : -0.1, 2,
308 0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE,
309 1,0,0, 1,0,0, 0,0);
310 joint = dJointCreateHinge (world,0);
311 dJointAttach (joint,body[0],body[1]);
312 dJointSetHingeAnchor (joint,0,0,1);
313 dJointSetHingeAxis (joint,0,1,0);
314 dJointSetHingeParam (joint,dParamLoStop,-0.9);
315 dJointSetHingeParam (joint,dParamHiStop,0.7854);
316 dJointSetHingeParam (joint,dParamBounce,0.5);
317 // anchor 2nd body with a fixed joint
318 dJointID j = dJointCreateFixed (world,0);
319 dJointAttach (j,body[1],0);
320 dJointSetFixed (j);
321 return 1;
322 }
323
324 // ********** slider
325
326 case 300: // 2 body
327 constructWorldForTest (0,2,
328 0,0,1, 0.2,0.2,1.2,
329 0,0,1, -1,1,0, 0,0.25*M_PI);
330 joint = dJointCreateSlider (world,0);
331 dJointAttach (joint,body[0],body[1]);
332 dJointSetSliderAxis (joint,1,1,1);
333 return 1;
334
335 case 320: // slider angle polarity test
336 case 321: // slider angle rate test
337 constructWorldForTest (0,2,
338 0,0,1, 0,0,1.2,
339 1,0,0, 1,0,0, 0,0);
340 joint = dJointCreateSlider (world,0);
341 dJointAttach (joint,body[0],body[1]);
342 dJointSetSliderAxis (joint,0,0,1);
343 max_iterations = 50;
344 return 1;
345
346 case 330: // slider motor rate (and polarity) test
347 case 331: // ...with stops
348 constructWorldForTest (0, 2,
349 0,0,1, 0,0,1.2,
350 1,0,0, 1,0,0, 0,0);
351 joint = dJointCreateSlider (world,0);
352 dJointAttach (joint,body[0],body[1]);
353 dJointSetSliderAxis (joint,0,0,1);
354 dJointSetSliderParam (joint,dParamFMax,100);
355 if (n==331) {
356 dJointSetSliderParam (joint,dParamLoStop,-0.4);
357 dJointSetSliderParam (joint,dParamHiStop,0.4);
358 }
359 return 1;
360
361 case 350: // limit bounce tests
362 case 351: {
363 constructWorldForTest ((n==351) ? 0.1 : -0.1, 2,
364 0,0,1, 0,0,1.2,
365 1,0,0, 1,0,0, 0,0);
366 joint = dJointCreateSlider (world,0);
367 dJointAttach (joint,body[0],body[1]);
368 dJointSetSliderAxis (joint,0,0,1);
369 dJointSetSliderParam (joint,dParamLoStop,-0.5);
370 dJointSetSliderParam (joint,dParamHiStop,0.5);
371 dJointSetSliderParam (joint,dParamBounce,0.5);
372 // anchor 2nd body with a fixed joint
373 dJointID j = dJointCreateFixed (world,0);
374 dJointAttach (j,body[1],0);
375 dJointSetFixed (j);
376 return 1;
377 }
378
379 // ********** hinge-2 joint
380
381 case 420: // hinge-2 steering angle polarity test
382 case 421: // hinge-2 steering angle rate test
383 constructWorldForTest (0,2,
384 0.5*SIDE,0,1, -0.5*SIDE,0,1,
385 1,0,0, 1,0,0, 0,0);
386 joint = dJointCreateHinge2 (world,0);
387 dJointAttach (joint,body[0],body[1]);
388 dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
389 dJointSetHinge2Axis1 (joint,0,0,1);
390 dJointSetHinge2Axis2 (joint,1,0,0);
391 max_iterations = 50;
392 return 1;
393
394 case 430: // hinge 2 steering motor rate (+polarity) test
395 case 431: // ...with stops
396 case 432: // hinge 2 wheel motor rate (+polarity) test
397 constructWorldForTest (0,2,
398 0.5*SIDE,0,1, -0.5*SIDE,0,1,
399 1,0,0, 1,0,0, 0,0);
400 joint = dJointCreateHinge2 (world,0);
401 dJointAttach (joint,body[0],body[1]);
402 dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
403 dJointSetHinge2Axis1 (joint,0,0,1);
404 dJointSetHinge2Axis2 (joint,1,0,0);
405 dJointSetHinge2Param (joint,dParamFMax,1);
406 dJointSetHinge2Param (joint,dParamFMax2,1);
407 if (n==431) {
408 dJointSetHinge2Param (joint,dParamLoStop,-0.5);
409 dJointSetHinge2Param (joint,dParamHiStop,0.5);
410 }
411 return 1;
412
413 // ********** angular motor joint
414
415 case 600: // test euler angle calculations
416 constructWorldForTest (0,2,
417 -SIDE*0.5,0,1, SIDE*0.5,0,1,
418 0,0,1, 0,0,1, 0,0);
419 joint = dJointCreateAMotor (world,0);
420 dJointAttach (joint,body[0],body[1]);
421
422 dJointSetAMotorNumAxes (joint,3);
423 dJointSetAMotorAxis (joint,0,1, 0,0,1);
424 dJointSetAMotorAxis (joint,2,2, 1,0,0);
425 dJointSetAMotorMode (joint,dAMotorEuler);
426 max_iterations = 200;
427 return 1;
428
429 // ********** universal joint
430
431 case 700: // 2 body
432 case 701:
433 case 702:
434 constructWorldForTest (0,2,
435 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
436 1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
437 joint = dJointCreateUniversal (world,0);
438 dJointAttach (joint,body[0],body[1]);
439 dJointSetUniversalAnchor (joint,0,0,1);
440 dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356);
441 dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356);
442 return 1;
443
444 case 720: // universal transmit torque test
445 case 721:
446 case 722:
447 case 730: // universal torque about axis 1
448 case 731:
449 case 732:
450 case 740: // universal torque about axis 2
451 case 741:
452 case 742:
453 constructWorldForTest (0,2,
454 0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
455 1,0,0, 1,0,0, 0,0);
456 joint = dJointCreateUniversal (world,0);
457 dJointAttach (joint,body[0],body[1]);
458 dJointSetUniversalAnchor (joint,0,0,1);
459 dJointSetUniversalAxis1 (joint,0,0,1);
460 dJointSetUniversalAxis2 (joint, 1, -1,0);
461 max_iterations = 100;
462 return 1;
463
464 // Joint PR (Prismatic and Rotoide)
465 case 800: // 2 body
466 case 801: // 2 bodies with spring force and prismatic fixed
467 case 802: // 2 bodies with torque on body1 and prismatic fixed
468 constructWorldForTest (0, 2,
469 -1.0, 0.0, 1.0,
470 1.0, 0.0, 1.0,
471 1,0,0, 1,0,0,
472 0, 0);
473 joint = dJointCreatePR (world, 0);
474 dJointAttach (joint, body[0], body[1]);
475 dJointSetPRAnchor (joint,-0.5, 0.0, 1.0);
476 dJointSetPRAxis1 (joint, 0, 1, 0);
477 dJointSetPRAxis2 (joint, 1, 0, 0);
478 dJointSetPRParam (joint,dParamLoStop,-0.5);
479 dJointSetPRParam (joint,dParamHiStop,0.5);
480 dJointSetPRParam (joint,dParamLoStop2,0);
481 dJointSetPRParam (joint,dParamHiStop2,0);
482 return 1;
483 case 803: // 2 bodies with spring force and prismatic NOT fixed
484 case 804: // 2 bodies with torque force and prismatic NOT fixed
485 case 805: // 2 bodies with force only on first body
486 constructWorldForTest (0, 2,
487 -1.0, 0.0, 1.0,
488 1.0, 0.0, 1.0,
489 1,0,0, 1,0,0,
490 0, 0);
491 joint = dJointCreatePR (world, 0);
492 dJointAttach (joint, body[0], body[1]);
493 dJointSetPRAnchor (joint,-0.5, 0.0, 1.0);
494 dJointSetPRAxis1 (joint, 0, 1, 0);
495 dJointSetPRAxis2 (joint, 1, 0, 0);
496 dJointSetPRParam (joint,dParamLoStop,-0.5);
497 dJointSetPRParam (joint,dParamHiStop,0.5);
498 dJointSetPRParam (joint,dParamLoStop2,-0.5);
499 dJointSetPRParam (joint,dParamHiStop2,0.5);
500 return 1;
501 }
502 return 0;
503 }
504
505
506 // do stuff specific to this test each iteration. you can check some
507 // invariants for the test -- the return value is some scaled error measurement
508 // that must be less than 1.
509 // return a dInfinity if error is not measured for this n.
510
doStuffAndGetError(int n)511 dReal doStuffAndGetError (int n)
512 {
513 switch (n) {
514
515 // ********** fixed joint
516
517 case 0: { // 2 body
518 addOscillatingTorque (0.1);
519 dampRotationalMotion (0.1);
520 // check the orientations are the same
521 const dReal *R1 = dBodyGetRotation (body[0]);
522 const dReal *R2 = dBodyGetRotation (body[1]);
523 dReal err1 = dMaxDifference (R1,R2,3,3);
524 // check the body offset is correct
525 dVector3 p,pp;
526 const dReal *p1 = dBodyGetPosition (body[0]);
527 const dReal *p2 = dBodyGetPosition (body[1]);
528 for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
529 dMultiply1_331 (pp,R1,p);
530 pp[0] += 0.5;
531 pp[1] += 0.5;
532 return (err1 + length (pp)) * 300;
533 }
534
535 case 1: { // 1 body to static env
536 addOscillatingTorque (0.1);
537
538 // check the orientation is the identity
539 dReal err1 = cmpIdentity (dBodyGetRotation (body[0]));
540
541 // check the body offset is correct
542 dVector3 p;
543 const dReal *p1 = dBodyGetPosition (body[0]);
544 for (int i=0; i<3; i++) p[i] = p1[i];
545 p[0] -= 0.25;
546 p[1] -= 0.25;
547 p[2] -= 1;
548 return (err1 + length (p)) * 1e6;
549 }
550
551 case 2: { // 2 body
552 addOscillatingTorque (0.1);
553 dampRotationalMotion (0.1);
554 // check the body offset is correct
555 // Should really check body rotation too. Oh well.
556 const dReal *R1 = dBodyGetRotation (body[0]);
557 dVector3 p,pp;
558 const dReal *p1 = dBodyGetPosition (body[0]);
559 const dReal *p2 = dBodyGetPosition (body[1]);
560 for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
561 dMultiply1_331 (pp,R1,p);
562 pp[0] += 0.5;
563 pp[1] += 0.5;
564 return length(pp) * 300;
565 }
566
567 case 3: { // 1 body to static env with relative rotation
568 addOscillatingTorque (0.1);
569
570 // check the body offset is correct
571 dVector3 p;
572 const dReal *p1 = dBodyGetPosition (body[0]);
573 for (int i=0; i<3; i++) p[i] = p1[i];
574 p[0] -= 0.25;
575 p[1] -= 0.25;
576 p[2] -= 1;
577 return length (p) * 1e6;
578 }
579
580
581 // ********** hinge joint
582
583 case 200: // 2 body
584 addOscillatingTorque (0.1);
585 dampRotationalMotion (0.1);
586 return dInfinity;
587
588 case 220: // hinge angle polarity test
589 dBodyAddTorque (body[0],0,0,0.01);
590 dBodyAddTorque (body[1],0,0,-0.01);
591 if (iteration == 40) {
592 dReal a = dJointGetHingeAngle (joint);
593 if (a > 0.5 && a < 1) return 0; else return 10;
594 }
595 return 0;
596
597 case 221: { // hinge angle rate test
598 static dReal last_angle = 0;
599 dBodyAddTorque (body[0],0,0,0.01);
600 dBodyAddTorque (body[1],0,0,-0.01);
601 dReal a = dJointGetHingeAngle (joint);
602 dReal r = dJointGetHingeAngleRate (joint);
603 dReal er = (a-last_angle)/STEPSIZE; // estimated rate
604 last_angle = a;
605 return fabs(r-er) * 4e4;
606 }
607
608 case 230: // hinge motor rate (and polarity) test
609 case 231: { // ...with stops
610 static dReal a = 0;
611 dReal r = dJointGetHingeAngleRate (joint);
612 dReal err = fabs (cos(a) - r);
613 if (a==0) err = 0;
614 a += 0.03;
615 dJointSetHingeParam (joint,dParamVel,cos(a));
616 if (n==231) return dInfinity;
617 return err * 1e6;
618 }
619
620 // ********** slider joint
621
622 case 300: // 2 body
623 addOscillatingTorque (0.05);
624 dampRotationalMotion (0.1);
625 addSpringForce (0.5);
626 return dInfinity;
627
628 case 320: // slider angle polarity test
629 dBodyAddForce (body[0],0,0,0.1);
630 dBodyAddForce (body[1],0,0,-0.1);
631 if (iteration == 40) {
632 dReal a = dJointGetSliderPosition (joint);
633 if (a > 0.2 && a < 0.5)
634 return 0;
635 else
636 return 10; // Failed
637 }
638 return 0;
639
640 case 321: { // slider angle rate test
641 static dReal last_pos = 0;
642 dBodyAddForce (body[0],0,0,0.1);
643 dBodyAddForce (body[1],0,0,-0.1);
644 dReal p = dJointGetSliderPosition (joint);
645 dReal r = dJointGetSliderPositionRate (joint);
646 dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact)
647 last_pos = p;
648 return fabs(r-er) * 1e9;
649 }
650
651 case 330: // slider motor rate (and polarity) test
652 case 331: { // ...with stops
653 static dReal a = 0;
654 dReal r = dJointGetSliderPositionRate (joint);
655 dReal err = fabs (0.7*cos(a) - r);
656 if (a < 0.04) err = 0;
657 a += 0.03;
658 dJointSetSliderParam (joint,dParamVel,0.7*cos(a));
659 if (n==331) return dInfinity;
660 return err * 1e6;
661 }
662
663 // ********** hinge-2 joint
664
665 case 420: // hinge-2 steering angle polarity test
666 dBodyAddTorque (body[0],0,0,0.01);
667 dBodyAddTorque (body[1],0,0,-0.01);
668 if (iteration == 40) {
669 dReal a = dJointGetHinge2Angle1 (joint);
670 if (a > 0.5 && a < 0.6) return 0; else return 10;
671 }
672 return 0;
673
674 case 421: { // hinge-2 steering angle rate test
675 static dReal last_angle = 0;
676 dBodyAddTorque (body[0],0,0,0.01);
677 dBodyAddTorque (body[1],0,0,-0.01);
678 dReal a = dJointGetHinge2Angle1 (joint);
679 dReal r = dJointGetHinge2Angle1Rate (joint);
680 dReal er = (a-last_angle)/STEPSIZE; // estimated rate
681 last_angle = a;
682 return fabs(r-er)*2e4;
683 }
684
685 case 430: // hinge 2 steering motor rate (+polarity) test
686 case 431: { // ...with stops
687 static dReal a = 0;
688 dReal r = dJointGetHinge2Angle1Rate (joint);
689 dReal err = fabs (cos(a) - r);
690 if (a==0) err = 0;
691 a += 0.03;
692 dJointSetHinge2Param (joint,dParamVel,cos(a));
693 if (n==431) return dInfinity;
694 return err * 1e6;
695 }
696
697 case 432: { // hinge 2 wheel motor rate (+polarity) test
698 static dReal a = 0;
699 dReal r = dJointGetHinge2Angle2Rate (joint);
700 dReal err = fabs (cos(a) - r);
701 if (a==0) err = 0;
702 a += 0.03;
703 dJointSetHinge2Param (joint,dParamVel2,cos(a));
704 return err * 1e6;
705 }
706
707 // ********** angular motor joint
708
709 case 600: { // test euler angle calculations
710 // desired euler angles from last iteration
711 static dReal a1,a2,a3;
712
713 // find actual euler angles
714 dReal aa1 = dJointGetAMotorAngle (joint,0);
715 dReal aa2 = dJointGetAMotorAngle (joint,1);
716 dReal aa3 = dJointGetAMotorAngle (joint,2);
717 // printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3);
718
719 dReal err = dInfinity;
720 if (iteration > 0) {
721 err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3);
722 err *= 1e10;
723 }
724
725 // get random base rotation for both bodies
726 dMatrix3 Rbase;
727 dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5),
728 3*(dRandReal()-0.5), 3*(dRandReal()-0.5));
729 dBodySetRotation (body[0],Rbase);
730
731 // rotate body 2 by random euler angles w.r.t. body 1
732 a1 = 3.14 * 2 * (dRandReal()-0.5);
733 a2 = 1.57 * 2 * (dRandReal()-0.5);
734 a3 = 3.14 * 2 * (dRandReal()-0.5);
735 dMatrix3 R1,R2,R3,Rtmp1,Rtmp2;
736 dRFromAxisAndAngle (R1,0,0,1,-a1);
737 dRFromAxisAndAngle (R2,0,1,0,a2);
738 dRFromAxisAndAngle (R3,1,0,0,-a3);
739 dMultiply0 (Rtmp1,R2,R3,3,3,3);
740 dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3);
741 dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3);
742 dBodySetRotation (body[1],Rtmp1);
743 // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);
744
745 return err;
746 }
747
748 // ********** universal joint
749
750 case 700: { // 2 body: joint constraint
751 dVector3 ax1, ax2;
752
753 addOscillatingTorque (0.1);
754 dampRotationalMotion (0.1);
755 dJointGetUniversalAxis1(joint, ax1);
756 dJointGetUniversalAxis2(joint, ax2);
757 return fabs(10*dCalcVectorDot3(ax1, ax2));
758 }
759
760 case 701: { // 2 body: angle 1 rate
761 static dReal last_angle = 0;
762 addOscillatingTorque (0.1);
763 dampRotationalMotion (0.1);
764 dReal a = dJointGetUniversalAngle1(joint);
765 dReal r = dJointGetUniversalAngle1Rate(joint);
766 dReal diff = a - last_angle;
767 if (diff > M_PI) diff -= 2*M_PI;
768 if (diff < -M_PI) diff += 2*M_PI;
769 dReal er = diff / STEPSIZE; // estimated rate
770 last_angle = a;
771 // I'm not sure why the error is so large here.
772 return fabs(r - er) * 1e1;
773 }
774
775 case 702: { // 2 body: angle 2 rate
776 static dReal last_angle = 0;
777 addOscillatingTorque (0.1);
778 dampRotationalMotion (0.1);
779 dReal a = dJointGetUniversalAngle2(joint);
780 dReal r = dJointGetUniversalAngle2Rate(joint);
781 dReal diff = a - last_angle;
782 if (diff > M_PI) diff -= 2*M_PI;
783 if (diff < -M_PI) diff += 2*M_PI;
784 dReal er = diff / STEPSIZE; // estimated rate
785 last_angle = a;
786 // I'm not sure why the error is so large here.
787 return fabs(r - er) * 1e1;
788 }
789
790 case 720: { // universal transmit torque test: constraint error
791 dVector3 ax1, ax2;
792 addOscillatingTorqueAbout (0.1, 1, 1, 0);
793 dampRotationalMotion (0.1);
794 dJointGetUniversalAxis1(joint, ax1);
795 dJointGetUniversalAxis2(joint, ax2);
796 return fabs(10*dCalcVectorDot3(ax1, ax2));
797 }
798
799 case 721: { // universal transmit torque test: angle1 rate
800 static dReal last_angle = 0;
801 addOscillatingTorqueAbout (0.1, 1, 1, 0);
802 dampRotationalMotion (0.1);
803 dReal a = dJointGetUniversalAngle1(joint);
804 dReal r = dJointGetUniversalAngle1Rate(joint);
805 dReal diff = a - last_angle;
806 if (diff > M_PI) diff -= 2*M_PI;
807 if (diff < -M_PI) diff += 2*M_PI;
808 dReal er = diff / STEPSIZE; // estimated rate
809 last_angle = a;
810 return fabs(r - er) * 1e10;
811 }
812
813 case 722: { // universal transmit torque test: angle2 rate
814 static dReal last_angle = 0;
815 addOscillatingTorqueAbout (0.1, 1, 1, 0);
816 dampRotationalMotion (0.1);
817 dReal a = dJointGetUniversalAngle2(joint);
818 dReal r = dJointGetUniversalAngle2Rate(joint);
819 dReal diff = a - last_angle;
820 if (diff > M_PI) diff -= 2*M_PI;
821 if (diff < -M_PI) diff += 2*M_PI;
822 dReal er = diff / STEPSIZE; // estimated rate
823 last_angle = a;
824 return fabs(r - er) * 1e10;
825 }
826
827 case 730:{
828 dVector3 ax1, ax2;
829 dJointGetUniversalAxis1(joint, ax1);
830 dJointGetUniversalAxis2(joint, ax2);
831 addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
832 dampRotationalMotion (0.1);
833 return fabs(10*dCalcVectorDot3(ax1, ax2));
834 }
835
836 case 731:{
837 dVector3 ax1;
838 static dReal last_angle = 0;
839 dJointGetUniversalAxis1(joint, ax1);
840 addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
841 dampRotationalMotion (0.1);
842 dReal a = dJointGetUniversalAngle1(joint);
843 dReal r = dJointGetUniversalAngle1Rate(joint);
844 dReal diff = a - last_angle;
845 if (diff > M_PI) diff -= 2*M_PI;
846 if (diff < -M_PI) diff += 2*M_PI;
847 dReal er = diff / STEPSIZE; // estimated rate
848 last_angle = a;
849 return fabs(r - er) * 2e3;
850 }
851
852 case 732:{
853 dVector3 ax1;
854 static dReal last_angle = 0;
855 dJointGetUniversalAxis1(joint, ax1);
856 addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
857 dampRotationalMotion (0.1);
858 dReal a = dJointGetUniversalAngle2(joint);
859 dReal r = dJointGetUniversalAngle2Rate(joint);
860 dReal diff = a - last_angle;
861 if (diff > M_PI) diff -= 2*M_PI;
862 if (diff < -M_PI) diff += 2*M_PI;
863 dReal er = diff / STEPSIZE; // estimated rate
864 last_angle = a;
865 return fabs(r - er) * 1e10;
866 }
867
868 case 740:{
869 dVector3 ax1, ax2;
870 dJointGetUniversalAxis1(joint, ax1);
871 dJointGetUniversalAxis2(joint, ax2);
872 addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
873 dampRotationalMotion (0.1);
874 return fabs(10*dCalcVectorDot3(ax1, ax2));
875 }
876
877 case 741:{
878 dVector3 ax2;
879 static dReal last_angle = 0;
880 dJointGetUniversalAxis2(joint, ax2);
881 addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
882 dampRotationalMotion (0.1);
883 dReal a = dJointGetUniversalAngle1(joint);
884 dReal r = dJointGetUniversalAngle1Rate(joint);
885 dReal diff = a - last_angle;
886 if (diff > M_PI) diff -= 2*M_PI;
887 if (diff < -M_PI) diff += 2*M_PI;
888 dReal er = diff / STEPSIZE; // estimated rate
889 last_angle = a;
890 return fabs(r - er) * 1e10;
891 }
892
893 case 742:{
894 dVector3 ax2;
895 static dReal last_angle = 0;
896 dJointGetUniversalAxis2(joint, ax2);
897 addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
898 dampRotationalMotion (0.1);
899 dReal a = dJointGetUniversalAngle2(joint);
900 dReal r = dJointGetUniversalAngle2Rate(joint);
901 dReal diff = a - last_angle;
902 if (diff > M_PI) diff -= 2*M_PI;
903 if (diff < -M_PI) diff += 2*M_PI;
904 dReal er = diff / STEPSIZE; // estimated rate
905 last_angle = a;
906 return fabs(r - er) * 1e4;
907 }
908
909 // ********** slider joint
910 case 801:
911 case 803:
912 addSpringForce (0.25);
913 return dInfinity;
914
915 case 802:
916 case 804: {
917 static dReal a = 0;
918 dBodyAddTorque (body[0], 0, 0.01*cos(1.5708*a), 0);
919 a += 0.01;
920 return dInfinity;
921 }
922
923 case 805:
924 addOscillatingForce (0.1);
925 return dInfinity;
926 }
927
928
929 return dInfinity;
930 }
931
932 //****************************************************************************
933 // simulation stuff common to all the tests
934
935 // start simulation - set viewpoint
936
start()937 static void start()
938 {
939 dAllocateODEDataForThread(dAllocateMaskAll);
940
941 static float xyz[3] = {1.0382f,-1.0811f,1.4700f};
942 static float hpr[3] = {135.0000f,-19.5000f,0.0000f};
943 dsSetViewpoint (xyz,hpr);
944 }
945
946
947 // simulation loop
948
simLoop(int pause)949 static void simLoop (int pause)
950 {
951 // stop after a given number of iterations, as long as we are not in
952 // interactive mode
953 if (cmd_graphics && !cmd_interactive &&
954 (iteration >= max_iterations)) {
955 dsStop();
956 return;
957 }
958 iteration++;
959
960 if (!pause) {
961 // do stuff for this test and check to see if the joint is behaving well
962 dReal error = doStuffAndGetError (test_num);
963 if (error > max_error) max_error = error;
964 if (cmd_interactive && error < dInfinity) {
965 printf ("scaled error = %.4e\n",error);
966 }
967
968 // take a step
969 dWorldStep (world,STEPSIZE);
970
971 // occasionally re-orient the first body to create a deliberate error.
972 if (cmd_occasional_error) {
973 static int count = 0;
974 if ((count % 20)==0) {
975 // randomly adjust orientation of body[0]
976 const dReal *R1;
977 dMatrix3 R2,R3;
978 R1 = dBodyGetRotation (body[0]);
979 dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
980 dRandReal()-0.5,dRandReal()-0.5);
981 dMultiply0 (R3,R1,R2,3,3,3);
982 dBodySetRotation (body[0],R3);
983
984 // randomly adjust position of body[0]
985 const dReal *pos = dBodyGetPosition (body[0]);
986 dBodySetPosition (body[0],
987 pos[0]+0.2*(dRandReal()-0.5),
988 pos[1]+0.2*(dRandReal()-0.5),
989 pos[2]+0.2*(dRandReal()-0.5));
990 }
991 count++;
992 }
993 }
994
995 if (cmd_graphics) {
996 dReal sides1[3] = {SIDE,SIDE,SIDE};
997 dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f};
998 dsSetTexture (DS_WOOD);
999 dsSetColor (1,1,0);
1000 dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
1001 if (body[1]) {
1002 dsSetColor (0,1,1);
1003 dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
1004 }
1005 }
1006 }
1007
1008 //****************************************************************************
1009 // conduct a specific test, and report the results
1010
doTest(int argc,char ** argv,int n,int fatal_if_bad_n)1011 void doTest (int argc, char **argv, int n, int fatal_if_bad_n)
1012 {
1013 test_num = n;
1014 iteration = 0;
1015 max_iterations = 300;
1016 max_error = 0;
1017
1018 if (! setupTest (n)) {
1019 if (fatal_if_bad_n) dError (0,"bad test number");
1020 return;
1021 }
1022
1023 // setup pointers to drawstuff callback functions
1024 dsFunctions fn;
1025 fn.version = DS_VERSION;
1026 fn.start = &start;
1027 fn.step = &simLoop;
1028 fn.command = 0;
1029 fn.stop = 0;
1030 if (cmd_path_to_textures)
1031 fn.path_to_textures = cmd_path_to_textures;
1032 else
1033 fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
1034
1035 // run simulation
1036 if (cmd_graphics) {
1037 dsSimulationLoop (argc,argv,352,288,&fn);
1038 }
1039 else {
1040 for (int i=0; i < max_iterations; i++) simLoop (0);
1041 }
1042 dWorldDestroy (world);
1043 body[0] = 0;
1044 body[1] = 0;
1045 joint = 0;
1046
1047 // print results
1048 printf ("test %d: ",n);
1049 if (max_error == dInfinity) printf ("error not computed\n");
1050 else {
1051 printf ("max scaled error = %.4e",max_error);
1052 if (max_error < 1) printf (" - passed\n");
1053 else printf (" - FAILED\n");
1054 }
1055 }
1056
1057 //****************************************************************************
1058 // main
1059
main(int argc,char ** argv)1060 int main (int argc, char **argv)
1061 {
1062 int i;
1063 dInitODE2(0);
1064
1065 // process the command line args. anything that starts with `-' is assumed
1066 // to be a drawstuff argument.
1067 for (i=1; i<argc; i++) {
1068 if ( argv[i][0]=='-' && argv[i][1]=='i' && argv[i][2]==0) cmd_interactive = 1;
1069 else if ( argv[i][0]=='-' && argv[i][1]=='g' && argv[i][2]==0) cmd_graphics = 0;
1070 else if ( argv[i][0]=='-' && argv[i][1]=='e' && argv[i][2]==0) cmd_graphics = 0;
1071 else if ( argv[i][0]=='-' && argv[i][1]=='n' && isdigit(argv[i][2]) ) {
1072 char *endptr;
1073 long int n = strtol (&(argv[i][2]),&endptr,10);
1074 if (*endptr == 0) cmd_test_num = n;
1075 }
1076 else
1077 cmd_path_to_textures = argv[i];
1078 }
1079
1080 // do the tests
1081 if (cmd_test_num == -1) {
1082 for (i=0; i<NUM_JOINTS*100; i++) doTest (argc,argv,i,0);
1083 }
1084 else {
1085 doTest (argc,argv,cmd_test_num,1);
1086 }
1087
1088 dCloseODE();
1089 return 0;
1090 }
1091