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