[216] | 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 | |
---|
| 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 |
---|
| 69 | static dWorldID world; |
---|
| 70 | static dBodyID body[2]; |
---|
| 71 | static dJointID joint; |
---|
| 72 | |
---|
| 73 | |
---|
| 74 | // data from the command line arguments |
---|
| 75 | static int cmd_test_num = -1; |
---|
| 76 | static int cmd_interactive = 0; |
---|
| 77 | static int cmd_graphics = 1; |
---|
| 78 | static char *cmd_path_to_textures = NULL; |
---|
| 79 | static int cmd_occasional_error = 0; // perturb occasionally |
---|
| 80 | |
---|
| 81 | |
---|
| 82 | // info about the current test |
---|
| 83 | struct TestInfo; |
---|
| 84 | static int test_num = 0; // number of the current test |
---|
| 85 | static int iteration = 0; |
---|
| 86 | static int max_iterations = 0; |
---|
| 87 | static dReal max_error = 0; |
---|
| 88 | |
---|
| 89 | //**************************************************************************** |
---|
| 90 | // utility stuff |
---|
| 91 | |
---|
| 92 | static char loCase (char a) |
---|
| 93 | { |
---|
| 94 | if (a >= 'A' && a <= 'Z') return a + ('a'-'A'); |
---|
| 95 | else return a; |
---|
| 96 | } |
---|
| 97 | |
---|
| 98 | |
---|
| 99 | static 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 | |
---|
| 107 | dReal 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 | |
---|
| 120 | void 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 | |
---|
| 157 | void 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 | |
---|
| 166 | void 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 | |
---|
| 177 | void 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 | |
---|
| 187 | void 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 | |
---|
| 198 | void 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 | |
---|
| 221 | int 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 | |
---|
| 517 | dReal 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 | |
---|
| 941 | static 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 | |
---|
| 951 | static 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 | |
---|
| 1013 | void 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 | |
---|
| 1062 | int 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 | } |
---|