applied patch #181 (AMotor bugfixes) from J. Cooper, with slight additions and unit tests

CHANGELOG.txt

@@ -8,6 +8,9 @@ the rules for this file:
   * keep the format consistent (79 char width, M/D/Y date format).
 
 ------------------------------------------------------------------------------
+12/06/13 Daniel K. O.
+        * Applied patch #181 to fix some bugs in AMotor.
+
 08/08/13 Oleh Derevenko
         * Joint feedback forces application fixed in QuickStep implementation
           (was broken since revision #1919 in old repository (#1927 in new one))

ode/src/joints/amotor.cpp

@@ -85,6 +85,12 @@ dxJointAMotor::computeGlobalAxes( dVector3 ax[3] )
                 if ( node[1].body )   // jds: don't assert, just ignore
                 {
                     dMultiply0_331( ax[i], node[1].body->posr.R, axis[i] );
+                } else
+                {
+                    // global - just copy it
+                    ax[i][0] = axis[i][0];
+                    ax[i][1] = axis[i][1];
+                    ax[i][2] = axis[i][2];
                 }
             }
             else
@@ -158,26 +164,16 @@ dxJointAMotor::setEulerReferenceVectors()
         dMultiply1_331( reference1, node[0].body->posr.R, r );
         dMultiply0_331( r, node[0].body->posr.R, axis[0] );
         dMultiply1_331( reference2, node[1].body->posr.R, r );
-    }
-
-    else     // jds
-    {
-        // else if (j->node[0].body) {
-        // dMultiply1_331 (j->reference1,j->node[0].body->posr.R,j->axis[2]);
-        // dMultiply0_331 (j->reference2,j->node[0].body->posr.R,j->axis[0]);
-
+    } else {
         // We want to handle angular motors attached to passive geoms
-        dVector3 r;  // axis[2] and axis[0] in global coordinates
-        r[0] = axis[2][0];
-        r[1] = axis[2][1];
-        r[2] = axis[2][2];
-        r[3] = axis[2][3];
-        dMultiply1_331( reference1, node[0].body->posr.R, r );
-        dMultiply0_331( r, node[0].body->posr.R, axis[0] );
-        reference2[0] += r[0];
-        reference2[1] += r[1];
-        reference2[2] += r[2];
-        reference2[3] += r[3];
+        // Replace missing node.R with identity
+        if (node[0].body) {
+          dMultiply1_331( reference1, node[0].body->posr.R, axis[2] );
+          dMultiply0_331( reference2, node[0].body->posr.R, axis[0] );
+        } else if (node[1].body) {
+          dMultiply0_331( reference1, node[1].body->posr.R, axis[2] );
+          dMultiply1_331( reference2, node[1].body->posr.R, axis[0] );
+        }
     }
 }
 
@@ -284,13 +280,13 @@ void dJointSetAMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal
     dxJointAMotor* joint = ( dxJointAMotor* )j;
     dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 );
     checktype( joint, AMotor );
-    dUASSERT( !( !joint->node[1].body && ( joint->flags & dJOINT_REVERSE ) && rel == 1 ), "no first body, can't set axis rel=1" );
-    dUASSERT( !( !joint->node[1].body && !( joint->flags & dJOINT_REVERSE ) && rel == 2 ), "no second body, can't set axis rel=2" );
+
     if ( anum < 0 ) anum = 0;
     if ( anum > 2 ) anum = 2;
 
     // adjust rel to match the internal body order
-    if ( !joint->node[1].body && rel == 2 ) rel = 1;
+    if ( (joint->flags & dJOINT_REVERSE) && rel )
+        rel ^= 3; // turns 1 into 2, 2, into 1
 
     joint->rel[anum] = rel;
 
@@ -307,7 +303,7 @@ void dJointSetAMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal
         {
             dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r );
         }
-        else
+        else // rel == 2
         {
             // don't assert; handle the case of attachment to a bodiless geom
             if ( joint->node[1].body )   // jds
@@ -316,6 +312,7 @@ void dJointSetAMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal
             }
             else
             {
+                printf("AAAA\n");
                 joint->axis[anum][0] = r[0];
                 joint->axis[anum][1] = r[1];
                 joint->axis[anum][2] = r[2];
@@ -325,6 +322,7 @@ void dJointSetAMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal
     }
     else
     {
+        printf("BBBB\n");
         joint->axis[anum][0] = r[0];
         joint->axis[anum][1] = r[1];
         joint->axis[anum][2] = r[2];
@@ -342,7 +340,7 @@ void dJointSetAMotorAngle( dJointID j, int anum, dReal angle )
     if ( joint->mode == dAMotorUser )
     {
         if ( anum < 0 ) anum = 0;
-        if ( anum > 3 ) anum = 3;
+        if ( anum > 2 ) anum = 2;
         joint->angle[anum] = angle;
     }
 }
@@ -391,8 +389,30 @@ void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
     checktype( joint, AMotor );
     if ( anum < 0 ) anum = 0;
     if ( anum > 2 ) anum = 2;
-    if ( joint->rel[anum] > 0 )
-    {
+    
+    // If we're in Euler mode, joint->axis[1] doesn't
+    // have anything sensible in it.  So don't just return
+    // that, find the actual effective axis.
+    // Likewise, the actual axis of rotation for the
+    // the other axes is different from what's stored.
+    if ( joint->mode == dAMotorEuler  ) {
+      dVector3 axes[3];
+      joint->computeGlobalAxes(axes);
+      if (anum == 1) {
+        result[0]=axes[1][0];
+        result[1]=axes[1][1];
+        result[2]=axes[1][2];
+      } else if (anum == 0) {
+        // This won't be unit length in general,
+        // but it's what's used in getInfo2
+        // This may be why things freak out as
+        // the body-relative axes get close to each other.
+        dCalcVectorCross3( result, axes[1], axes[2] );
+      } else if (anum == 2) {
+        // Same problem as above.
+        dCalcVectorCross3( result, axes[0], axes[1] );
+      }
+    } else if ( joint->rel[anum] > 0 ) {
         if ( joint->rel[anum] == 1 )
         {
             dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
@@ -428,7 +448,10 @@ int dJointGetAMotorAxisRel( dJointID j, int anum )
     checktype( joint, AMotor );
     if ( anum < 0 ) anum = 0;
     if ( anum > 2 ) anum = 2;
-    return joint->rel[anum];
+    int rel = joint->rel[anum];
+    if ( (joint->flags & dJOINT_REVERSE) && rel)
+         rel ^= 3; // turns 1 into 2, 2 into 1
+    return rel;
 }
 
 
@@ -438,16 +461,24 @@ dReal dJointGetAMotorAngle( dJointID j, int anum )
     dAASSERT( joint && anum >= 0 && anum < 3 );
     checktype( joint, AMotor );
     if ( anum < 0 ) anum = 0;
-    if ( anum > 3 ) anum = 3;
+    if ( anum > 2 ) anum = 2;
     return joint->angle[anum];
 }
 
 
 dReal dJointGetAMotorAngleRate( dJointID j, int anum )
 {
-    //dxJointAMotor* joint = (dxJointAMotor*)j;
-    // @@@
-    dDebug( 0, "not yet implemented" );
+    dxJointAMotor* joint = ( dxJointAMotor* )j;
+    dAASSERT( joint && anum >= 0 && anum < 3);
+    checktype( joint, AMotor );
+  
+    if (joint->node[0].body) {
+      dVector3 axis;
+      dJointGetAMotorAxis (joint, anum, axis);
+      dReal rate = dDOT(axis,joint->node[0].body->avel);
+      if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
+      return rate;
+    }
     return 0;
 }
 

tests/Makefile.am

@@ -25,6 +25,7 @@ tests_SOURCES = \
                 joint.cpp \
                 main.cpp \
                 odemath.cpp \
+                joints/amotor.cpp \
                 joints/ball.cpp \
                 joints/fixed.cpp \
                 joints/hinge.cpp \

tests/joints/amotor.cpp

@@ -0,0 +1,324 @@
+/*************************************************************************
+  *                                                                       *
+  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+  * All rights reserved.  Email: [email protected]   Web: www.q12.org          *
+  *                                                                       *
+  * This library is free software; you can redistribute it and/or         *
+  * modify it under the terms of EITHER:                                  *
+  *   (1) The GNU Lesser General Public License as published by the Free  *
+  *       Software Foundation; either version 2.1 of the License, or (at  *
+  *       your option) any later version. The text of the GNU Lesser      *
+  *       General Public License is included with this library in the     *
+  *       file LICENSE.TXT.                                               *
+  *   (2) The BSD-style license that is included with this library in     *
+  *       the file LICENSE-BSD.TXT.                                       *
+  *                                                                       *
+  * This library is distributed in the hope that it will be useful,       *
+  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+  *                                                                       *
+  *************************************************************************/
+//234567890123456789012345678901234567890123456789012345678901234567890123456789
+//        1         2         3         4         5         6         7
+
+////////////////////////////////////////////////////////////////////////////////
+// This file create unit test for some of the functions found in:
+// ode/src/joinst/fixed.cpp
+//
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <UnitTest++.h>
+#include <ode/ode.h>
+
+#include "config.h"
+#include "../../ode/src/joints/amotor.h"
+
+const dReal tol = 1e-5;
+
+SUITE (TestdxJointAMotor)
+{
+    struct FixtureBase {
+        dWorldID world;
+        dBodyID body;
+        dJointID joint;
+
+        FixtureBase()
+        {
+            world = dWorldCreate();
+            body = dBodyCreate(world);
+            joint = dJointCreateAMotor(world, 0);
+        }
+
+        ~FixtureBase()
+        {
+            dJointDestroy(joint);
+            dBodyDestroy(body);
+            dWorldDestroy(world);
+        }
+    };
+
+
+    struct FixtureXUser: FixtureBase {
+        FixtureXUser()
+        {
+            // body only allowed to rotate around X axis
+            dBodySetFiniteRotationMode(body, 1);
+            dBodySetFiniteRotationAxis(body, 1, 0, 0);
+            dJointAttach(joint, body, 0);
+            dJointSetAMotorNumAxes(joint, 2);
+            dJointSetAMotorAxis(joint, 0, 2, 0, 1, 0);
+            dJointSetAMotorAxis(joint, 1, 2, 0, 0, 1);
+            dJointSetAMotorParam(joint, dParamVel, 0);
+            dJointSetAMotorParam(joint, dParamFMax, dInfinity);
+            dJointSetAMotorParam(joint, dParamVel2, 0);
+            dJointSetAMotorParam(joint, dParamFMax2, dInfinity);
+        }
+    };
+
+    TEST_FIXTURE(FixtureXUser, rotate_x)
+    {
+        const dReal h = 1;
+        const dReal v = 1;
+        dMatrix3 identity = {1, 0, 0, 0,
+                             0, 1, 0, 0,
+                             0, 0, 1, 0};
+        dBodySetRotation(body, identity);
+        dBodySetAngularVel(body, v, 0, 0);
+        dWorldQuickStep(world, h);
+        const dReal* rot = dBodyGetRotation(body);
+        CHECK_CLOSE(1, rot[0], tol);
+        CHECK_CLOSE(0, rot[4], tol);
+        CHECK_CLOSE(0, rot[8], tol);
+
+        CHECK_CLOSE(0, rot[1], tol);
+        CHECK_CLOSE(dCos(v*h), rot[5], tol);
+        CHECK_CLOSE(dSin(v*h), rot[9], tol);
+
+        CHECK_CLOSE(0, rot[2], tol);
+        CHECK_CLOSE(-dSin(v*h), rot[6], tol);
+        CHECK_CLOSE( dCos(v*h), rot[10], tol);
+    }
+
+    TEST_FIXTURE(FixtureXUser, rotate_yz)
+    {
+        const dReal h = 1;
+        const dReal v = 1;
+        dMatrix3 identity = {1, 0, 0, 0,
+                             0, 1, 0, 0,
+                             0, 0, 1, 0};
+        dBodySetRotation(body, identity);
+
+        dVector3 axis_y;
+        dJointGetAMotorAxis(joint, 0, axis_y);
+        CHECK_CLOSE(0, axis_y[0], tol);
+        CHECK_CLOSE(1, axis_y[1], tol);
+        CHECK_CLOSE(0, axis_y[2], tol);
+
+        dVector3 axis_z;
+        dJointGetAMotorAxis(joint, 1, axis_z);
+        CHECK_CLOSE(0, axis_z[0], tol);
+        CHECK_CLOSE(0, axis_z[1], tol);
+        CHECK_CLOSE(1, axis_z[2], tol);
+
+        dBodySetAngularVel(body, 0, v, v);
+        dWorldStep(world, h);
+        const dReal* rot = dBodyGetRotation(body);
+        CHECK_CLOSE(1, rot[0], tol);
+        CHECK_CLOSE(0, rot[4], tol);
+        CHECK_CLOSE(0, rot[8], tol);
+
+        CHECK_CLOSE(0, rot[1], tol);
+        CHECK_CLOSE(1, rot[5], tol);
+        CHECK_CLOSE(0, rot[9], tol);
+
+        CHECK_CLOSE(0, rot[2], tol);
+        CHECK_CLOSE(0, rot[6], tol);
+        CHECK_CLOSE(1, rot[10], tol);
+    }
+
+
+    TEST_FIXTURE(FixtureBase, sanity_check)
+    {
+        dMatrix3 R;
+        dRFromAxisAndAngle(R, 1, 1, 1, 10*M_PI/180);
+        dBodySetRotation(body, R);
+
+        dVector3 res;
+
+        dJointAttach(joint, body, 0);
+        dJointSetAMotorNumAxes(joint, 3);
+        CHECK_EQUAL(3, dJointGetAMotorNumAxes(joint));
+
+        // axes relative to world
+        dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0);
+        dJointGetAMotorAxis(joint, 0, res);
+        CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 0));
+        CHECK_CLOSE(1, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 1, 0, 0, 1, 0);
+        dJointGetAMotorAxis(joint, 1, res);
+        CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 1));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(1, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1);
+        dJointGetAMotorAxis(joint, 2, res);
+        CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 2));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(1, res[2], tol);
+
+        // axes relative to body1
+        dJointSetAMotorAxis(joint, 0, 1, 1, 0, 0);
+        dJointGetAMotorAxis(joint, 0, res);
+        CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 0));
+        CHECK_CLOSE(1, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 1, 1, 0, 1, 0);
+        dJointGetAMotorAxis(joint, 1, res);
+        CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 1));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(1, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 2, 1, 0, 0, 1);
+        dJointGetAMotorAxis(joint, 2, res);
+        CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 2));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(1, res[2], tol);
+
+        // axes relative to body2
+        dJointSetAMotorAxis(joint, 0, 2, 1, 0, 0);
+        dJointGetAMotorAxis(joint, 0, res);
+        CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 0));
+        CHECK_CLOSE(1, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 1, 2, 0, 1, 0);
+        dJointGetAMotorAxis(joint, 1, res);
+        CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 1));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(1, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 2, 2, 0, 0, 1);
+        dJointGetAMotorAxis(joint, 2, res);
+        CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 2));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(1, res[2], tol);
+
+        // reverse attachment to force internal reversal
+        dJointAttach(joint, 0, body);
+        // axes relative to world
+        dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0);
+        dJointGetAMotorAxis(joint, 0, res);
+        CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 0));
+        CHECK_CLOSE(1, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 1, 0, 0, 1, 0);
+        dJointGetAMotorAxis(joint, 1, res);
+        CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 1));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(1, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1);
+        dJointGetAMotorAxis(joint, 2, res);
+        CHECK_EQUAL(0, dJointGetAMotorAxisRel(joint, 2));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(1, res[2], tol);
+
+        // axes relative to body1
+        dJointSetAMotorAxis(joint, 0, 1, 1, 0, 0);
+        dJointGetAMotorAxis(joint, 0, res);
+        CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 0));
+        CHECK_CLOSE(1, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 1, 1, 0, 1, 0);
+        dJointGetAMotorAxis(joint, 1, res);
+        CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 1));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(1, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 2, 1, 0, 0, 1);
+        dJointGetAMotorAxis(joint, 2, res);
+        CHECK_EQUAL(1, dJointGetAMotorAxisRel(joint, 2));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(1, res[2], tol);
+
+        // axes relative to body2
+        dJointSetAMotorAxis(joint, 0, 2, 1, 0, 0);
+        dJointGetAMotorAxis(joint, 0, res);
+        CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 0));
+        CHECK_CLOSE(1, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 1, 2, 0, 1, 0);
+        dJointGetAMotorAxis(joint, 1, res);
+        CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 1));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(1, res[1], tol);
+        CHECK_CLOSE(0, res[2], tol);
+
+        dJointSetAMotorAxis(joint, 2, 2, 0, 0, 1);
+        dJointGetAMotorAxis(joint, 2, res);
+        CHECK_EQUAL(2, dJointGetAMotorAxisRel(joint, 2));
+        CHECK_CLOSE(0, res[0], tol);
+        CHECK_CLOSE(0, res[1], tol);
+        CHECK_CLOSE(1, res[2], tol);
+    }
+
+
+    struct FixtureXEuler : FixtureBase {
+        FixtureXEuler()
+        {
+            // body only allowed to rotate around X axis
+            dJointAttach(joint, 0, body);
+            dJointSetAMotorMode(joint, dAMotorEuler);
+            dJointSetAMotorAxis(joint, 0, 0, 1, 0, 0);
+            dJointSetAMotorAxis(joint, 2, 0, 0, 0, 1);
+        }
+    };
+
+
+    TEST_FIXTURE(FixtureXEuler, check_axes)
+    {
+        // test patch #181 bug fix
+        dVector3 axis_x;
+        dJointGetAMotorAxis(joint, 0, axis_x);
+        CHECK_CLOSE(1, axis_x[0], tol);
+        CHECK_CLOSE(0, axis_x[1], tol);
+        CHECK_CLOSE(0, axis_x[2], tol);
+
+        dVector3 axis_y;
+        dJointGetAMotorAxis(joint, 1, axis_y);
+        CHECK_CLOSE(0, axis_y[0], tol);
+        CHECK_CLOSE(1, axis_y[1], tol);
+        CHECK_CLOSE(0, axis_y[2], tol);
+
+        dVector3 axis_z;
+        dJointGetAMotorAxis(joint, 2, axis_z);
+        CHECK_CLOSE(0, axis_z[0], tol);
+        CHECK_CLOSE(0, axis_z[1], tol);
+        CHECK_CLOSE(1, axis_z[2], tol);
+    }
+
+} // End of SUITE TestdxJointAMotor