[ODE] Universal Joint

Martin C. Martin martin at metahuman.org
Thu Apr 4 17:01:01 2002


This is a multi-part message in MIME format.
--------------11B0806D95DE09F80FA53187
Content-Type: text/plain; charset=us-ascii
Content-Transfer-Encoding: 7bit

Hey all,

Here's an implementation of a Universal joint.  It doesn't have a motor
or joint limits, but it does come with tests.

Enjoy,
Martin
--------------11B0806D95DE09F80FA53187
Content-Type: text/plain; charset=us-ascii;
 name="diffs"
Content-Transfer-Encoding: 7bit
Content-Disposition: inline;
 filename="diffs"

diff -c -r ode-0.03/include/ode/common.h ode-universal/include/ode/common.h
*** ode-0.03/include/ode/common.h	Mon Dec 24 23:29:06 2001
--- ode-universal/include/ode/common.h	Thu Apr  4 15:31:36 2002
***************
*** 198,203 ****
--- 198,204 ----
    dJointTypeHinge,
    dJointTypeSlider,
    dJointTypeContact,
+   dJointTypeUniversal,
    dJointTypeHinge2,
    dJointTypeFixed,
    dJointTypeNull,
Only in ode-universal/include/ode: config.h
diff -c -r ode-0.03/include/ode/objects.h ode-universal/include/ode/objects.h
*** ode-0.03/include/ode/objects.h	Mon Dec 24 23:29:06 2001
--- ode-universal/include/ode/objects.h	Thu Apr  4 16:05:16 2002
***************
*** 107,112 ****
--- 107,113 ----
  dJointID dJointCreateSlider (dWorldID, dJointGroupID);
  dJointID dJointCreateContact (dWorldID, dJointGroupID, const dContact *);
  dJointID dJointCreateHinge2 (dWorldID, dJointGroupID);
+ dJointID dJointCreateUniversal (dWorldID, dJointGroupID);
  dJointID dJointCreateFixed (dWorldID, dJointGroupID);
  dJointID dJointCreateNull (dWorldID, dJointGroupID);
  dJointID dJointCreateAMotor (dWorldID, dJointGroupID);
***************
*** 133,138 ****
--- 134,142 ----
  void dJointSetHinge2Axis1 (dJointID, dReal x, dReal y, dReal z);
  void dJointSetHinge2Axis2 (dJointID, dReal x, dReal y, dReal z);
  void dJointSetHinge2Param (dJointID, int parameter, dReal value);
+ void dJointSetUniversalAnchor (dJointID, dReal x, dReal y, dReal z);
+ void dJointSetUniversalAxis1 (dJointID, dReal x, dReal y, dReal z);
+ void dJointSetUniversalAxis2 (dJointID, dReal x, dReal y, dReal z);
  void dJointSetFixed (dJointID);
  void dJointSetAMotorNumAxes (dJointID, int num);
  void dJointSetAMotorAxis (dJointID, int anum, int rel,
***************
*** 158,163 ****
--- 162,170 ----
  dReal dJointGetHinge2Angle1 (dJointID);
  dReal dJointGetHinge2Angle1Rate (dJointID);
  dReal dJointGetHinge2Angle2Rate (dJointID);
+ void dJointGetUniversalAnchor (dJointID, dVector3 result);
+ void dJointGetUniversalAxis1 (dJointID, dVector3 result);
+ void dJointGetUniversalAxis2 (dJointID, dVector3 result);
  int dJointGetAMotorNumAxes (dJointID);
  void dJointGetAMotorAxis (dJointID, int anum, dVector3 result);
  int dJointGetAMotorAxisRel (dJointID, int anum);
Only in ode-universal: lib
diff -c -r ode-0.03/ode/src/joint.cpp ode-universal/ode/src/joint.cpp
*** ode-0.03/ode/src/joint.cpp	Mon Dec 24 23:29:06 2001
--- ode-universal/ode/src/joint.cpp	Thu Apr  4 18:49:20 2002
***************
*** 1511,1516 ****
--- 1511,1686 ----
    (dxJoint::getInfo2_fn*) hinge2GetInfo2,
    dJointTypeHinge2};
  
+ 
+ //****************************************************************************
+ // universal
+ 
+ static void universalInit (dxJointUniversal *j)
+ {
+   dSetZero (j->anchor1,4);
+   dSetZero (j->anchor2,4);
+   dSetZero (j->axis1,4);
+   j->axis1[0] = 1;
+   dSetZero (j->axis2,4);
+   j->axis2[1] = 1;
+ }
+ 
+ 
+ static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info)
+ {
+   info->nub = 4;
+   info->m = 4;
+ }
+ 
+ 
+ static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info)
+ {
+   // set the three ball-and-socket rows
+   setBall (joint,info,joint->anchor1,joint->anchor2);
+ 
+   // set the universal join row.  the angular velocity about an axis perpendicular
+   // to both joint axes should be equal.  thus the constraint equation is
+   //    p*w1 - p*w2 = 0
+   // where p is a vector normal to both joint axes, and w1 and w2
+   // are the angular velocity vectors of the two bodies.
+ 
+   dVector3 ax1, ax2; // length 1 joint axis in global coordinates, from each body
+   dVector3 p; // length 1 vector perpendicular to ax1 and ax2.  Neither body can rotate about this.
+   
+   // This says "ax1 = joint->node[0].body->R * joint->axis1"
+   dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
+   if (joint->node[1].body) {
+     dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
+   }
+   else {
+     ax2[0] = joint->axis2[0];
+     ax2[1] = joint->axis2[1];
+     ax2[2] = joint->axis2[2];
+   }
+ 
+   // if ax1 and ax2 are almost parallel, p won't be perpendicular to them.
+   // Is there some more robust way to do this?
+   dCROSS(p, =, ax1, ax2);
+   dNormalize3(p);
+  
+   int s3=3*info->rowskip;
+ 
+   info->J1a[s3+0] = p[0];
+   info->J1a[s3+1] = p[1];
+   info->J1a[s3+2] = p[2];
+ 
+   if (joint->node[1].body) {
+     info->J2a[s3+0] = -p[0];
+     info->J2a[s3+1] = -p[1];
+     info->J2a[s3+2] = -p[2];
+   }
+ 
+ 
+   // compute the right hand side of the constraint equation. set relative
+   // body velocities along p to bring the axes back to perpendicular.
+   // If ax1, ax2 are unit length joint axes as computed from body1 and
+   // body2, we need to rotate both bodies along the axis p.  If theta
+   // is the angle between ax1 and ax2, we need an angular velocity
+   // along p to cover the angle erp * (theta - Pi/2) in one step:
+   //
+   //   |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
+   //                      = (erp*fps) * (theta - Pi/2)
+   //
+   // if theta is close to Pi/2, 
+   // theta - Pi/2 ~= cos(theta), so
+   //    |angular_velocity|  = (erp*fps) * (ax1 dot ax2)
+ 
+   info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2);
+ }
+ 
+ 
+ 
+ extern "C" void dJointSetUniversalAnchor (dxJointUniversal *joint,
+ 				      dReal x, dReal y, dReal z)
+ {
+   dUASSERT(joint,"bad joint argument");
+   dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+   setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
+ }
+ 
+ 
+ extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint,
+ 				    dReal x, dReal y, dReal z)
+ {
+   dUASSERT(joint,"bad joint argument");
+   dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+   if (joint->node[0].body) {
+     dReal q[4];
+     q[0] = x;
+     q[1] = y;
+     q[2] = z;
+     q[3] = 0;
+     dNormalize3 (q);
+     dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q);
+   }
+   joint->axis1[3] = 0;
+ }
+ 
+ 
+ extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint,
+ 				    dReal x, dReal y, dReal z)
+ {
+   dUASSERT(joint,"bad joint argument");
+   dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+   if (joint->node[1].body) {
+     dReal q[4];
+     q[0] = x;
+     q[1] = y;
+     q[2] = z;
+     q[3] = 0;
+     dNormalize3 (q);
+     dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q);
+   }
+   joint->axis2[3] = 0;
+ }
+ 
+ 
+ extern "C" void dJointGetUniversalAnchor (dxJointUniversal *joint, dVector3 result)
+ {
+   dUASSERT(joint,"bad joint argument");
+   dUASSERT(result,"bad result argument");
+   dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+   getAnchor (joint,result,joint->anchor1);
+ }
+ 
+ 
+ extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint, dVector3 result)
+ {
+   dUASSERT(joint,"bad joint argument");
+   dUASSERT(result,"bad result argument");
+   dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+   if (joint->node[0].body) {
+     dMULTIPLY0_331 (result, joint->node[0].body->R, joint->axis1);
+   }
+ }
+ 
+ 
+ extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint, dVector3 result)
+ {
+   dUASSERT(joint,"bad joint argument");
+   dUASSERT(result,"bad result argument");
+   dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+   if (joint->node[1].body) {
+     dMULTIPLY0_331 (result, joint->node[1].body->R, joint->axis2);
+   }
+ }
+ 
+ 
+ 
+ dxJoint::Vtable __duniversal_vtable = {
+   sizeof(dxJointUniversal),
+   (dxJoint::init_fn*) universalInit,
+   (dxJoint::getInfo1_fn*) universalGetInfo1,
+   (dxJoint::getInfo2_fn*) universalGetInfo2,
+   dJointTypeUniversal};
+ 
+ 
+ 
  //****************************************************************************
  // angular motor
  
diff -c -r ode-0.03/ode/src/joint.h ode-universal/ode/src/joint.h
*** ode-0.03/ode/src/joint.h	Mon Dec 24 23:29:06 2001
--- ode-universal/ode/src/joint.h	Thu Apr  4 15:30:08 2002
***************
*** 175,180 ****
--- 175,191 ----
  extern struct dxJoint::Vtable __dhinge_vtable;
  
  
+ // universal
+ 
+ struct dxJointUniversal : public dxJoint {
+   dVector3 anchor1;		// anchor w.r.t first body
+   dVector3 anchor2;		// anchor w.r.t second body
+   dVector3 axis1;		// axis w.r.t first body
+   dVector3 axis2;		// axis w.r.t second body
+ };
+ extern struct dxJoint::Vtable __duniversal_vtable;
+ 
+ 
  // slider. if body2 is 0 then qrel is the absolute rotation of body1 and
  // offset is the position of body1 center along axis1.
  
diff -c -r ode-0.03/ode/src/ode.cpp ode-universal/ode/src/ode.cpp
*** ode-0.03/ode/src/ode.cpp	Mon Dec 24 23:29:06 2001
--- ode-universal/ode/src/ode.cpp	Thu Apr  4 15:58:34 2002
***************
*** 836,841 ****
--- 836,848 ----
  }
  
  
+ dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group)
+ {
+   dAASSERT (w);
+   return createJoint (w,group,&__duniversal_vtable);
+ }
+ 
+ 
  dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group)
  {
    dAASSERT (w);
diff -c -r ode-0.03/ode/test/test_joints.cpp ode-universal/ode/test/test_joints.cpp
*** ode-0.03/ode/test/test_joints.cpp	Mon Dec 24 23:29:08 2001
--- ode-universal/ode/test/test_joints.cpp	Thu Apr  4 18:53:10 2002
***************
*** 161,166 ****
--- 161,173 ----
    a += 0.01;
  }
  
+ void addOscillatingTorqueAbout(dReal tscale, dReal x, dReal y, dReal z)
+ {
+   static dReal a=0;
+   dBodyAddTorque (body[0], tscale*cos(a) * x, tscale*cos(a) * y, tscale * cos(a) * z);
+   a += 0.02;
+ }
+ 
  
  // damp the rotational motion of body 0 a bit
  
***************
*** 192,197 ****
--- 199,205 ----
  //   4xx : hinge 2
  //   5xx : contact
  //   6xx : amotor
+ //   7xx : universal joint
  
  
  // setup for the given test. return 0 if there is no such test
***************
*** 387,392 ****
--- 395,427 ----
      dJointSetAMotorMode (joint,dAMotorEuler);
      max_iterations = 200;
      return 1;
+ 
+   // ********** universal joint
+ 
+   case 700:			// 2 body
+     constructWorldForTest (0,2,
+ 			   0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
+ 			   1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
+     joint = dJointCreateUniversal (world,0);
+     dJointAttach (joint,body[0],body[1]);
+     dJointSetUniversalAnchor (joint,0,0,1);
+     dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356);
+     dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356);
+     return 1;
+ 
+   case 720:		// universal transmit torque test
+   case 730:		// universal torque about axis 1
+   case 740:		// universal torque about axis 2
+     constructWorldForTest (0,2,
+ 			   0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
+ 			   1,0,0, 1,0,0, 0,0);
+     joint = dJointCreateUniversal (world,0);
+     dJointAttach (joint,body[0],body[1]);
+     dJointSetUniversalAnchor (joint,0,0,1);
+     dJointSetUniversalAxis1 (joint,0,0,1);
+     dJointSetUniversalAxis2 (joint, 1, -1,0);
+     max_iterations = 50;
+     return 1;
    }
    return 0;
  }
***************
*** 600,605 ****
--- 635,679 ----
      // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);
  
      return err;
+   }
+ 
+   // ********** universal joint
+ 
+   case 700: {		// 2 body
+     dVector3 ax1, ax2;
+ 
+     addOscillatingTorque (0.1);
+     dampRotationalMotion (0.1);
+     dJointGetUniversalAxis1(joint, ax1);
+     dJointGetUniversalAxis2(joint, ax2);
+     return fabs(10*dDOT(ax1, ax2));
+   }
+ 
+   case 720:{		// universal transmit torque test
+     dVector3 ax1, ax2;
+     addOscillatingTorqueAbout (0.1, 1, 1, 0);
+     dampRotationalMotion (0.1);
+     dJointGetUniversalAxis1(joint, ax1);
+     dJointGetUniversalAxis2(joint, ax2);
+     return fabs(10*dDOT(ax1, ax2));
+   }
+ 
+   case 730:{
+     dVector3 ax1, ax2;
+     dJointGetUniversalAxis1(joint, ax1);
+     dJointGetUniversalAxis2(joint, ax2);
+     addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
+     dampRotationalMotion (0.1);
+     return fabs(10*dDOT(ax1, ax2));
+   }
+ 
+   case 740:{
+     dVector3 ax1, ax2;
+     dJointGetUniversalAxis1(joint, ax1);
+     dJointGetUniversalAxis2(joint, ax2);
+     addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
+     dampRotationalMotion (0.1);
+     return fabs(10*dDOT(ax1, ax2));
    }
    }
  

--------------11B0806D95DE09F80FA53187--