[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--