[ODE] value passed to dAddTorque isn't actually torque?

[Martin C. Martin]

Hi,

The picture at:

http://www.ai.mit.edu/~mcm/BlockAndHinge.jpg

shows a block in mid air, free to fall except for a
hinge on one end (the red line).  If you do the math, you can show that
gravity produces a torque about the hinge's axis of:

g M l/2

where l is the length of the box.  Therefore, if I apply an equal torque
in the opposite direction, it should perfectly balance and the thing
shouldn't move.  However, that's not what happens.  Instead, to balance
it, I need the following torque:

g 1/2

That is, same as you'd expect, but without the mass.  That formula works
for changes in:

- acceleration from gravity
- box density
- box length
- box width
- box height

Is this a bug in ODE?  It says that the torque needed to support a rod at
one end is the same whether its made from lead or Styrofoam.  That seems
wrong.  The source I used is below.  What can be done to fix/work around
this?

Thanks,
Martin

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

// select correct drawing functions

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCappedCylinder dsDrawCappedCylinderD
#endif

#define box_length (1.0f)
#define box_width (0.5f)
#define box_height (0.5f)
#define DENSITY (5.0f)
#define box_z (1.5f)

#define grav (9.8f)

static dWorldID world;
static dSpaceID space;
static dJointGroupID contactgroup;

static dGeomID box_geom;
static dBodyID box_body;
static dJointID box_hinge;

// This torque will be applied every timestep.
dReal torque;

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
}

static void start()
{
  static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
  static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
  dsSetViewpoint (xyz,hpr);
}


static void simLoop (int pause)
{
  const dReal *pos;
  const dReal *R;
  dVector3 sides, hinge_pos, hinge_axis;
  dMatrix3 hinge_R;

  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) {
	  dBodyAddTorque(box_body, 0, - torque, 0);
	  dWorldStep (world,0.01);
  }

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  // Draw the box
  dsSetColor (0,0,0.5);
  dsSetTexture (DS_NONE);

  pos = dGeomGetPosition(box_geom);
  R = dGeomGetRotation(box_geom);
  dGeomBoxGetLengths(box_geom, sides);
  dsDrawBox(pos, R, sides);

  // Draw the hinge
  dsSetColor(1, 0, 0);
  dJointGetHingeAnchor(box_hinge, hinge_pos);
  dJointGetHingeAxis(box_hinge, hinge_axis);
  dRFromZAxis(hinge_R, hinge_axis[0], hinge_axis[1], hinge_axis[2]);
  dsDrawCylinder(hinge_pos, hinge_R, 1.0, 0.02);
}


int main (int argc, char **argv)
{
  dMass mass;
  dReal moment;

  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = NULL;
  fn.stop = 0;
  fn.path_to_textures = "textures";

  // create world

  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-grav);
  dWorldSetCFM (world,1e-5);

  box_geom = dCreateBox(space, box_length, box_width, box_height);

  box_body = dBodyCreate(world);
  dGeomSetBody(box_geom, box_body);
  dBodySetPosition(box_body, box_length/2, 0, box_z);
  dMassSetBox(&mass, DENSITY, box_length, box_width, box_height);

  box_hinge = dJointCreateHinge(world, 0);
  dJointAttach(box_hinge, box_body, NULL);
  dJointSetHingeAnchor(box_hinge, 0, 0, box_z);
  dJointSetHingeAxis(box_hinge, 0, 1, 0);

  moment = mass.I[1*4+1] + mass.mass*box_length * box_length / 4;
  printf("Moment about COM: %f\n", moment);
 
//  torque = grav * mass.mass * box_length / 2;
  torque = grav * box_length / 2;

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);

  return 0;
}